|
0
|
1 /**
|
|
|
2 * Stone of Orthanc
|
|
|
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
|
|
|
4 * Department, University Hospital of Liege, Belgium
|
|
|
5 *
|
|
|
6 * This program is free software: you can redistribute it and/or
|
|
|
7 * modify it under the terms of the GNU General Public License as
|
|
|
8 * published by the Free Software Foundation, either version 3 of the
|
|
|
9 * License, or (at your option) any later version.
|
|
|
10 *
|
|
|
11 * In addition, as a special exception, the copyright holders of this
|
|
|
12 * program give permission to link the code of its release with the
|
|
|
13 * OpenSSL project's "OpenSSL" library (or with modified versions of it
|
|
|
14 * that use the same license as the "OpenSSL" library), and distribute
|
|
|
15 * the linked executables. You must obey the GNU General Public License
|
|
|
16 * in all respects for all of the code used other than "OpenSSL". If you
|
|
|
17 * modify file(s) with this exception, you may extend this exception to
|
|
|
18 * your version of the file(s), but you are not obligated to do so. If
|
|
|
19 * you do not wish to do so, delete this exception statement from your
|
|
|
20 * version. If you delete this exception statement from all source files
|
|
|
21 * in the program, then also delete it here.
|
|
|
22 *
|
|
|
23 * This program is distributed in the hope that it will be useful, but
|
|
|
24 * WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
25 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
|
26 * General Public License for more details.
|
|
|
27 *
|
|
|
28 * You should have received a copy of the GNU General Public License
|
|
|
29 * along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
30 **/
|
|
|
31
|
|
|
32
|
|
|
33 #include "GeometryToolbox.h"
|
|
|
34
|
|
16
|
35 #include "../../Resources/Orthanc/Core/OrthancException.h"
|
|
|
36 #include "../../Resources/Orthanc/Core/Toolbox.h"
|
|
0
|
37
|
|
|
38 #include <stdio.h>
|
|
|
39 #include <boost/lexical_cast.hpp>
|
|
|
40
|
|
|
41 namespace OrthancStone
|
|
|
42 {
|
|
|
43 namespace GeometryToolbox
|
|
|
44 {
|
|
|
45 void Print(const Vector& v)
|
|
|
46 {
|
|
|
47 for (size_t i = 0; i < v.size(); i++)
|
|
|
48 {
|
|
|
49 printf("%8.2f\n", v[i]);
|
|
|
50 }
|
|
|
51 printf("\n");
|
|
|
52 }
|
|
|
53
|
|
|
54
|
|
|
55 bool ParseVector(Vector& target,
|
|
|
56 const std::string& value)
|
|
|
57 {
|
|
|
58 std::vector<std::string> items;
|
|
|
59 Orthanc::Toolbox::TokenizeString(items, value, '\\');
|
|
|
60
|
|
|
61 target.resize(items.size());
|
|
|
62
|
|
|
63 for (size_t i = 0; i < items.size(); i++)
|
|
|
64 {
|
|
|
65 try
|
|
|
66 {
|
|
|
67 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i]));
|
|
|
68 }
|
|
|
69 catch (boost::bad_lexical_cast&)
|
|
|
70 {
|
|
|
71 target.clear();
|
|
|
72 return false;
|
|
|
73 }
|
|
|
74 }
|
|
|
75
|
|
|
76 return true;
|
|
|
77 }
|
|
|
78
|
|
|
79
|
|
|
80 void AssignVector(Vector& v,
|
|
|
81 double v1,
|
|
|
82 double v2)
|
|
|
83 {
|
|
|
84 v.resize(2);
|
|
|
85 v[0] = v1;
|
|
|
86 v[1] = v2;
|
|
|
87 }
|
|
|
88
|
|
|
89
|
|
|
90 void AssignVector(Vector& v,
|
|
|
91 double v1,
|
|
|
92 double v2,
|
|
|
93 double v3)
|
|
|
94 {
|
|
|
95 v.resize(3);
|
|
|
96 v[0] = v1;
|
|
|
97 v[1] = v2;
|
|
|
98 v[2] = v3;
|
|
|
99 }
|
|
|
100
|
|
|
101
|
|
|
102 bool IsNear(double x,
|
|
|
103 double y)
|
|
|
104 {
|
|
|
105 // As most input is read as single-precision numbers, we take the
|
|
|
106 // epsilon machine for float32 into consideration to compare numbers
|
|
|
107 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon());
|
|
|
108 }
|
|
|
109
|
|
|
110
|
|
|
111 void NormalizeVector(Vector& u)
|
|
|
112 {
|
|
|
113 double norm = boost::numeric::ublas::norm_2(u);
|
|
|
114 if (!IsCloseToZero(norm))
|
|
|
115 {
|
|
|
116 u = u / norm;
|
|
|
117 }
|
|
|
118 }
|
|
|
119
|
|
|
120
|
|
|
121 void CrossProduct(Vector& result,
|
|
|
122 const Vector& u,
|
|
|
123 const Vector& v)
|
|
|
124 {
|
|
|
125 if (u.size() != 3 ||
|
|
|
126 v.size() != 3)
|
|
|
127 {
|
|
|
128 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
|
|
|
129 }
|
|
|
130
|
|
|
131 result.resize(3);
|
|
|
132
|
|
|
133 result[0] = u[1] * v[2] - u[2] * v[1];
|
|
|
134 result[1] = u[2] * v[0] - u[0] * v[2];
|
|
|
135 result[2] = u[0] * v[1] - u[1] * v[0];
|
|
|
136 }
|
|
|
137
|
|
|
138
|
|
|
139 void ProjectPointOntoPlane(Vector& result,
|
|
|
140 const Vector& point,
|
|
|
141 const Vector& planeNormal,
|
|
|
142 const Vector& planeOrigin)
|
|
|
143 {
|
|
|
144 double norm = boost::numeric::ublas::norm_2(planeNormal);
|
|
|
145 if (IsCloseToZero(norm))
|
|
|
146 {
|
|
|
147 // Division by zero
|
|
|
148 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
|
|
|
149 }
|
|
|
150
|
|
|
151 // Make sure the norm of the normal is 1
|
|
|
152 Vector n;
|
|
|
153 n = planeNormal / norm;
|
|
|
154
|
|
|
155 // Algebraic form of line–plane intersection, where the line passes
|
|
|
156 // through "point" along the direction "normal" (thus, l == n)
|
|
|
157 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form
|
|
|
158 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point;
|
|
|
159 }
|
|
|
160
|
|
|
161
|
|
|
162
|
|
|
163 bool IsParallelOrOpposite(bool& isOpposite,
|
|
|
164 const Vector& u,
|
|
|
165 const Vector& v)
|
|
|
166 {
|
|
|
167 // The dot product of the two vectors gives the cosine of the angle
|
|
|
168 // between the vectors
|
|
|
169 // https://en.wikipedia.org/wiki/Dot_product
|
|
|
170
|
|
|
171 double normU = boost::numeric::ublas::norm_2(u);
|
|
|
172 double normV = boost::numeric::ublas::norm_2(v);
|
|
|
173
|
|
|
174 if (IsCloseToZero(normU) ||
|
|
|
175 IsCloseToZero(normV))
|
|
|
176 {
|
|
|
177 return false;
|
|
|
178 }
|
|
|
179
|
|
|
180 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV);
|
|
|
181
|
|
|
182 // The angle must be zero, so the cosine must be almost equal to
|
|
|
183 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true)
|
|
|
184
|
|
|
185 if (IsCloseToZero(cosAngle - 1.0))
|
|
|
186 {
|
|
|
187 isOpposite = false;
|
|
|
188 return true;
|
|
|
189 }
|
|
|
190 else if (IsCloseToZero(fabs(cosAngle) - 1.0))
|
|
|
191 {
|
|
|
192 isOpposite = true;
|
|
|
193 return true;
|
|
|
194 }
|
|
|
195 else
|
|
|
196 {
|
|
|
197 return false;
|
|
|
198 }
|
|
|
199 }
|
|
|
200
|
|
|
201
|
|
|
202 bool IsParallel(const Vector& u,
|
|
|
203 const Vector& v)
|
|
|
204 {
|
|
|
205 bool isOpposite;
|
|
|
206 return (IsParallelOrOpposite(isOpposite, u, v) &&
|
|
|
207 !isOpposite);
|
|
|
208 }
|
|
|
209
|
|
|
210
|
|
|
211 bool IntersectTwoPlanes(Vector& p,
|
|
|
212 Vector& direction,
|
|
|
213 const Vector& origin1,
|
|
|
214 const Vector& normal1,
|
|
|
215 const Vector& origin2,
|
|
|
216 const Vector& normal2)
|
|
|
217 {
|
|
|
218 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane
|
|
|
219 // Intersect Point" of:
|
|
|
220 // http://geomalgorithms.com/a05-_intersect-1.html
|
|
|
221
|
|
|
222 // The direction of the line of intersection is orthogonal to the
|
|
|
223 // normal of both planes
|
|
|
224 CrossProduct(direction, normal1, normal2);
|
|
|
225
|
|
|
226 double norm = boost::numeric::ublas::norm_2(direction);
|
|
|
227 if (IsCloseToZero(norm))
|
|
|
228 {
|
|
|
229 // The two planes are parallel or coincident
|
|
|
230 return false;
|
|
|
231 }
|
|
|
232
|
|
|
233 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1);
|
|
|
234 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2);
|
|
|
235 Vector tmp = d2 * normal1 - d1 * normal2;
|
|
|
236
|
|
|
237 CrossProduct(p, tmp, direction);
|
|
|
238 p /= norm;
|
|
|
239
|
|
|
240 return true;
|
|
|
241 }
|
|
|
242
|
|
|
243
|
|
|
244 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out)
|
|
|
245 double& y1,
|
|
|
246 double& x2,
|
|
|
247 double& y2,
|
|
|
248 const double ax, // Two points defining the line (in)
|
|
|
249 const double ay,
|
|
|
250 const double bx,
|
|
|
251 const double by,
|
|
|
252 const double& xmin, // Coordinates of the rectangle (in)
|
|
|
253 const double& ymin,
|
|
|
254 const double& xmax,
|
|
|
255 const double& ymax)
|
|
|
256 {
|
|
|
257 // This is Skala algorithm for rectangles, "A new approach to line
|
|
|
258 // and line segment clipping in homogeneous coordinates"
|
|
|
259 // (2005). This is a direct, non-optimized translation of Algorithm
|
|
|
260 // 2 in the paper.
|
|
|
261
|
|
|
262 static uint8_t tab1[16] = { 255 /* none */,
|
|
|
263 0,
|
|
|
264 0,
|
|
|
265 1,
|
|
|
266 1,
|
|
|
267 255 /* na */,
|
|
|
268 0,
|
|
|
269 2,
|
|
|
270 2,
|
|
|
271 0,
|
|
|
272 255 /* na */,
|
|
|
273 1,
|
|
|
274 1,
|
|
|
275 0,
|
|
|
276 0,
|
|
|
277 255 /* none */ };
|
|
|
278
|
|
|
279
|
|
|
280 static uint8_t tab2[16] = { 255 /* none */,
|
|
|
281 3,
|
|
|
282 1,
|
|
|
283 3,
|
|
|
284 2,
|
|
|
285 255 /* na */,
|
|
|
286 2,
|
|
|
287 3,
|
|
|
288 3,
|
|
|
289 2,
|
|
|
290 255 /* na */,
|
|
|
291 2,
|
|
|
292 3,
|
|
|
293 1,
|
|
|
294 3,
|
|
|
295 255 /* none */ };
|
|
|
296
|
|
|
297 // Create the coordinates of the rectangle
|
|
|
298 Vector x[4];
|
|
|
299 AssignVector(x[0], xmin, ymin, 1.0);
|
|
|
300 AssignVector(x[1], xmax, ymin, 1.0);
|
|
|
301 AssignVector(x[2], xmax, ymax, 1.0);
|
|
|
302 AssignVector(x[3], xmin, ymax, 1.0);
|
|
|
303
|
|
|
304 // Move to homogoneous coordinates in 2D
|
|
|
305 Vector p;
|
|
|
306
|
|
|
307 {
|
|
|
308 Vector a, b;
|
|
|
309 AssignVector(a, ax, ay, 1.0);
|
|
|
310 AssignVector(b, bx, by, 1.0);
|
|
|
311 CrossProduct(p, a, b);
|
|
|
312 }
|
|
|
313
|
|
|
314 uint8_t c = 0;
|
|
|
315
|
|
|
316 for (unsigned int k = 0; k < 4; k++)
|
|
|
317 {
|
|
|
318 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0)
|
|
|
319 {
|
|
|
320 c |= (1 << k);
|
|
|
321 }
|
|
|
322 }
|
|
|
323
|
|
|
324 assert(c < 16);
|
|
|
325
|
|
|
326 uint8_t i = tab1[c];
|
|
|
327 uint8_t j = tab2[c];
|
|
|
328
|
|
|
329 if (i == 255 || j == 255)
|
|
|
330 {
|
|
|
331 return false; // No intersection
|
|
|
332 }
|
|
|
333 else
|
|
|
334 {
|
|
|
335 Vector a, b, e;
|
|
|
336 CrossProduct(e, x[i], x[(i + 1) % 4]);
|
|
|
337 CrossProduct(a, p, e);
|
|
|
338 CrossProduct(e, x[j], x[(j + 1) % 4]);
|
|
|
339 CrossProduct(b, p, e);
|
|
|
340
|
|
|
341 // Go back to non-homogeneous coordinates
|
|
|
342 x1 = a[0] / a[2];
|
|
|
343 y1 = a[1] / a[2];
|
|
|
344 x2 = b[0] / b[2];
|
|
|
345 y2 = b[1] / b[2];
|
|
|
346
|
|
|
347 return true;
|
|
|
348 }
|
|
|
349 }
|
|
|
350 }
|
|
|
351 }
|
