Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/GeometryToolbox.cpp @ 1063:3e8f59cefec6 broker
integration mainline->broker
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Tue, 15 Oct 2019 12:22:44 +0200 |
parents | 53cc787bd7bc |
children | 2d8ab34c8c91 |
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 | |
1013
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
55 /* |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
56 undefined results if vector are not 3D |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
57 */ |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
58 void ProjectPointOntoPlane2( |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
59 double& resultX, |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
60 double& resultY, |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
61 double& resultZ, |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
62 const Vector& point, |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
63 const Vector& planeNormal, |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
64 const Vector& planeOrigin) |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
65 { |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
66 double pointX = point[0]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
67 double pointY = point[1]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
68 double pointZ = point[2]; |
0 | 69 |
1013
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
70 double planeNormalX = planeNormal[0]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
71 double planeNormalY = planeNormal[1]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
72 double planeNormalZ = planeNormal[2]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
73 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
74 double planeOriginX = planeOrigin[0]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
75 double planeOriginY = planeOrigin[1]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
76 double planeOriginZ = planeOrigin[2]; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
77 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
78 double normSq = (planeNormalX * planeNormalX) + (planeNormalY * planeNormalY) + (planeNormalZ * planeNormalZ); |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
79 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
80 // Algebraic form of line–plane intersection, where the line passes |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
81 // through "point" along the direction "normal" (thus, l == n) |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
82 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
83 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
84 if (LinearAlgebra::IsNear(1.0, normSq)) |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
85 { |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
86 double nX = planeNormalX; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
87 double nY = planeNormalY; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
88 double nZ = planeNormalZ; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
89 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
90 double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
91 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
92 resultX = prod * nX + pointX; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
93 resultY = prod * nY + pointY; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
94 resultZ = prod * nZ + pointZ; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
95 } |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
96 else |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
97 { |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
98 double norm = sqrt(normSq); |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
99 if (LinearAlgebra::IsCloseToZero(norm)) |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
100 { |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
101 // Division by zero |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
102 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
103 } |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
104 double invNorm = 1.0 / norm; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
105 double nX = planeNormalX * invNorm; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
106 double nY = planeNormalY * invNorm; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
107 double nZ = planeNormalZ * invNorm; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
108 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
109 double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
110 |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
111 resultX = prod * nX + pointX; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
112 resultY = prod * nY + pointY; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
113 resultZ = prod * nZ + pointZ; |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
114 } |
53cc787bd7bc
- Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents:
439
diff
changeset
|
115 } |
0 | 116 |
117 bool IsParallelOrOpposite(bool& isOpposite, | |
118 const Vector& u, | |
119 const Vector& v) | |
120 { | |
121 // The dot product of the two vectors gives the cosine of the angle | |
122 // between the vectors | |
123 // https://en.wikipedia.org/wiki/Dot_product | |
124 | |
125 double normU = boost::numeric::ublas::norm_2(u); | |
126 double normV = boost::numeric::ublas::norm_2(v); | |
127 | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
128 if (LinearAlgebra::IsCloseToZero(normU) || |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
129 LinearAlgebra::IsCloseToZero(normV)) |
0 | 130 { |
131 return false; | |
132 } | |
133 | |
134 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); | |
135 | |
136 // The angle must be zero, so the cosine must be almost equal to | |
137 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) | |
138 | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
139 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0)) |
0 | 140 { |
141 isOpposite = false; | |
142 return true; | |
143 } | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
144 else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0)) |
0 | 145 { |
146 isOpposite = true; | |
147 return true; | |
148 } | |
149 else | |
150 { | |
151 return false; | |
152 } | |
153 } | |
154 | |
155 | |
156 bool IsParallel(const Vector& u, | |
157 const Vector& v) | |
158 { | |
159 bool isOpposite; | |
160 return (IsParallelOrOpposite(isOpposite, u, v) && | |
161 !isOpposite); | |
162 } | |
163 | |
164 | |
165 bool IntersectTwoPlanes(Vector& p, | |
166 Vector& direction, | |
167 const Vector& origin1, | |
168 const Vector& normal1, | |
169 const Vector& origin2, | |
170 const Vector& normal2) | |
171 { | |
172 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane | |
173 // Intersect Point" of: | |
174 // http://geomalgorithms.com/a05-_intersect-1.html | |
175 | |
176 // The direction of the line of intersection is orthogonal to the | |
177 // normal of both planes | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
178 LinearAlgebra::CrossProduct(direction, normal1, normal2); |
0 | 179 |
180 double norm = boost::numeric::ublas::norm_2(direction); | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
181 if (LinearAlgebra::IsCloseToZero(norm)) |
0 | 182 { |
183 // The two planes are parallel or coincident | |
184 return false; | |
185 } | |
186 | |
187 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); | |
188 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); | |
189 Vector tmp = d2 * normal1 - d1 * normal2; | |
190 | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
191 LinearAlgebra::CrossProduct(p, tmp, direction); |
0 | 192 p /= norm; |
193 | |
194 return true; | |
195 } | |
196 | |
197 | |
198 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out) | |
199 double& y1, | |
200 double& x2, | |
201 double& y2, | |
202 const double ax, // Two points defining the line (in) | |
203 const double ay, | |
204 const double bx, | |
205 const double by, | |
206 const double& xmin, // Coordinates of the rectangle (in) | |
207 const double& ymin, | |
208 const double& xmax, | |
209 const double& ymax) | |
210 { | |
211 // This is Skala algorithm for rectangles, "A new approach to line | |
212 // and line segment clipping in homogeneous coordinates" | |
213 // (2005). This is a direct, non-optimized translation of Algorithm | |
214 // 2 in the paper. | |
215 | |
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
216 static const uint8_t tab1[16] = { 255 /* none */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
217 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
218 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
219 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
220 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
221 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
222 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
223 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
224 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
225 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
226 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
227 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
228 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
229 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
230 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
231 255 /* none */ }; |
0 | 232 |
233 | |
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
234 static const uint8_t tab2[16] = { 255 /* none */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
235 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
236 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
237 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
238 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
239 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
240 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
241 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
242 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
243 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
244 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
245 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
246 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
247 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
248 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
249 255 /* none */ }; |
0 | 250 |
251 // Create the coordinates of the rectangle | |
252 Vector x[4]; | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
253 LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
254 LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
255 LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
256 LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0); |
0 | 257 |
258 // Move to homogoneous coordinates in 2D | |
259 Vector p; | |
260 | |
261 { | |
262 Vector a, b; | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
263 LinearAlgebra::AssignVector(a, ax, ay, 1.0); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
264 LinearAlgebra::AssignVector(b, bx, by, 1.0); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
265 LinearAlgebra::CrossProduct(p, a, b); |
0 | 266 } |
267 | |
268 uint8_t c = 0; | |
269 | |
270 for (unsigned int k = 0; k < 4; k++) | |
271 { | |
272 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0) | |
273 { | |
274 c |= (1 << k); | |
275 } | |
276 } | |
277 | |
278 assert(c < 16); | |
279 | |
280 uint8_t i = tab1[c]; | |
281 uint8_t j = tab2[c]; | |
282 | |
283 if (i == 255 || j == 255) | |
284 { | |
285 return false; // No intersection | |
286 } | |
287 else | |
288 { | |
289 Vector a, b, e; | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
290 LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
291 LinearAlgebra::CrossProduct(a, p, e); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
292 LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]); |
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
293 LinearAlgebra::CrossProduct(b, p, e); |
0 | 294 |
295 // Go back to non-homogeneous coordinates | |
296 x1 = a[0] / a[2]; | |
297 y1 = a[1] / a[2]; | |
298 x2 = b[0] / b[2]; | |
299 y2 = b[1] / b[2]; | |
300 | |
301 return true; | |
302 } | |
303 } | |
32 | 304 |
305 | |
306 void GetPixelSpacing(double& spacingX, | |
307 double& spacingY, | |
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
308 const Orthanc::DicomMap& dicom) |
32 | 309 { |
310 Vector v; | |
311 | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
312 if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) |
32 | 313 { |
314 if (v.size() != 2 || | |
315 v[0] <= 0 || | |
316 v[1] <= 0) | |
317 { | |
318 LOG(ERROR) << "Bad value for PixelSpacing tag"; | |
319 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat); | |
320 } | |
321 else | |
322 { | |
363
54ae0577f5bb
fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
212
diff
changeset
|
323 // WARNING: X/Y are swapped (Y comes first) |
54ae0577f5bb
fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
212
diff
changeset
|
324 spacingX = v[1]; |
54ae0577f5bb
fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
212
diff
changeset
|
325 spacingY = v[0]; |
32 | 326 } |
327 } | |
328 else | |
329 { | |
330 // The "PixelSpacing" is of type 1C: It could be absent, use | |
331 // default value in such a case | |
332 spacingX = 1; | |
333 spacingY = 1; | |
334 } | |
335 } | |
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
336 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
337 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
338 Matrix CreateRotationMatrixAlongX(double a) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
339 { |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
340 // Rotate along X axis (R_x) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
341 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
342 Matrix r(3, 3); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
343 r(0,0) = 1; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
344 r(0,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
345 r(0,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
346 r(1,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
347 r(1,1) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
348 r(1,2) = -sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
349 r(2,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
350 r(2,1) = sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
351 r(2,2) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
352 return r; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
353 } |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
354 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
355 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
356 Matrix CreateRotationMatrixAlongY(double a) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
357 { |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
358 // Rotate along Y axis (R_y) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
359 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
360 Matrix r(3, 3); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
361 r(0,0) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
362 r(0,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
363 r(0,2) = sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
364 r(1,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
365 r(1,1) = 1; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
366 r(1,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
367 r(2,0) = -sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
368 r(2,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
369 r(2,2) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
370 return r; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
371 } |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
372 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
373 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
374 Matrix CreateRotationMatrixAlongZ(double a) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
375 { |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
376 // Rotate along Z axis (R_z) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
377 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
378 Matrix r(3, 3); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
379 r(0,0) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
380 r(0,1) = -sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
381 r(0,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
382 r(1,0) = sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
383 r(1,1) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
384 r(1,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
385 r(2,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
386 r(2,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
387 r(2,2) = 1; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
388 return r; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
389 } |
151
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
390 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
391 |
165
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
392 Matrix CreateTranslationMatrix(double dx, |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
393 double dy, |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
394 double dz) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
395 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
396 Matrix m = LinearAlgebra::IdentityMatrix(4); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
397 m(0,3) = dx; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
398 m(1,3) = dy; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
399 m(2,3) = dz; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
400 return m; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
401 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
402 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
403 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
404 Matrix CreateScalingMatrix(double sx, |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
405 double sy, |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
406 double sz) |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
407 { |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
408 Matrix m = LinearAlgebra::IdentityMatrix(4); |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
409 m(0,0) = sx; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
410 m(1,1) = sy; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
411 m(2,2) = sz; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
412 return m; |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
413 } |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
414 |
8d50e6be565d
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
158
diff
changeset
|
415 |
151
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
416 bool IntersectPlaneAndSegment(Vector& p, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
417 const Vector& normal, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
418 double d, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
419 const Vector& edgeFrom, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
420 const Vector& edgeTo) |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
421 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
422 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
423 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
424 // Check for parallel line and plane |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
425 Vector direction = edgeTo - edgeFrom; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
426 double denominator = boost::numeric::ublas::inner_prod(direction, normal); |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
427 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
428 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
429 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
430 return false; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
431 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
432 else |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
433 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
434 // Compute intersection |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
435 double t = -(normal[0] * edgeFrom[0] + |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
436 normal[1] * edgeFrom[1] + |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
437 normal[2] * edgeFrom[2] + d) / denominator; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
438 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
439 if (t >= 0 && t <= 1) |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
440 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
441 // The intersection lies inside edge segment |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
442 p = edgeFrom + t * direction; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
443 return true; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
444 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
445 else |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
446 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
447 return false; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
448 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
449 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
450 } |
156 | 451 |
452 | |
157
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
453 bool IntersectPlaneAndLine(Vector& p, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
454 const Vector& normal, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
455 double d, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
456 const Vector& origin, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
457 const Vector& direction) |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
458 { |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
459 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
460 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
461 // Check for parallel line and plane |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
462 double denominator = boost::numeric::ublas::inner_prod(direction, normal); |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
463 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
464 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
465 { |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
466 return false; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
467 } |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
468 else |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
469 { |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
470 // Compute intersection |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
471 double t = -(normal[0] * origin[0] + |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
472 normal[1] * origin[1] + |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
473 normal[2] * origin[2] + d) / denominator; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
474 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
475 p = origin + t * direction; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
476 return true; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
477 } |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
478 } |
189
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
479 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
480 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
481 void AlignVectorsWithRotation(Matrix& r, |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
482 const Vector& a, |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
483 const Vector& b) |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
484 { |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
485 // This is Rodrigues' rotation formula: |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
486 // 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
|
487 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
488 // 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
|
489 // Vision - 2nd edition" (p. 584) |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
490 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
491 if (a.size() != 3 || |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
492 b.size() != 3) |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
493 { |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
494 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
495 } |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
496 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
497 double aNorm = boost::numeric::ublas::norm_2(a); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
498 double bNorm = boost::numeric::ublas::norm_2(b); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
499 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
500 if (LinearAlgebra::IsCloseToZero(aNorm) || |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
501 LinearAlgebra::IsCloseToZero(bNorm)) |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
502 { |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
503 LOG(ERROR) << "Vector with zero norm"; |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
504 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
505 } |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
506 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
507 Vector aUnit, bUnit; |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
508 aUnit = a / aNorm; |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
509 bUnit = b / bNorm; |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
510 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
511 Vector v; |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
512 LinearAlgebra::CrossProduct(v, aUnit, bUnit); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
513 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
514 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
515 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
516 if (LinearAlgebra::IsCloseToZero(1 + cosine)) |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
517 { |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
518 // "a == -b": TODO |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
519 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
520 } |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
521 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
522 Matrix k; |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
523 LinearAlgebra::CreateSkewSymmetric(k, v); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
524 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
525 #if 0 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
526 double sine = boost::numeric::ublas::norm_2(v); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
527 |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
528 r = (boost::numeric::ublas::identity_matrix<double>(3) + |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
529 sine * k + |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
530 (1 - cosine) * boost::numeric::ublas::prod(k, k)); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
531 #else |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
532 r = (boost::numeric::ublas::identity_matrix<double>(3) + |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
533 k + |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
534 boost::numeric::ublas::prod(k, k) / (1 + cosine)); |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
535 #endif |
964118e7e6de
unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
165
diff
changeset
|
536 } |
0 | 537 } |
538 } |