Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/GeometryToolbox.cpp @ 150:62670cc2bb50 wasm
print matrix
| author | Sebastien Jodogne <s.jodogne@gmail.com> |
|---|---|
| date | Thu, 01 Feb 2018 16:07:54 +0100 |
| parents | 2115530d3703 |
| children | c5044bbfc303 |
| 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 { | |
| 39 printf("%8.2f\n", v[i]); | |
| 40 } | |
| 41 printf("\n"); | |
| 42 } | |
| 43 | |
| 44 | |
| 150 | 45 void Print(const Matrix& m) |
| 46 { | |
| 47 for (size_t i = 0; i < m.size1(); i++) | |
| 48 { | |
| 49 for (size_t j = 0; j < m.size2(); j++) | |
| 50 { | |
| 51 printf("%8.2f ", m(i,j)); | |
| 52 } | |
| 53 printf("\n"); | |
| 54 } | |
| 55 } | |
| 56 | |
| 57 | |
| 0 | 58 bool ParseVector(Vector& target, |
| 59 const std::string& value) | |
| 60 { | |
| 61 std::vector<std::string> items; | |
| 62 Orthanc::Toolbox::TokenizeString(items, value, '\\'); | |
| 63 | |
| 64 target.resize(items.size()); | |
| 65 | |
| 66 for (size_t i = 0; i < items.size(); i++) | |
| 67 { | |
| 68 try | |
| 69 { | |
| 70 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); | |
| 71 } | |
| 72 catch (boost::bad_lexical_cast&) | |
| 73 { | |
| 74 target.clear(); | |
| 75 return false; | |
| 76 } | |
| 77 } | |
| 78 | |
| 79 return true; | |
| 80 } | |
| 81 | |
| 82 | |
| 32 | 83 bool ParseVector(Vector& target, |
|
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
84 const Orthanc::DicomMap& dataset, |
|
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
85 const Orthanc::DicomTag& tag) |
| 32 | 86 { |
| 87 std::string value; | |
|
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
88 return (dataset.CopyToString(value, tag, false) && |
| 32 | 89 ParseVector(target, value)); |
| 90 } | |
| 91 | |
| 92 | |
| 0 | 93 void AssignVector(Vector& v, |
| 94 double v1, | |
| 95 double v2) | |
| 96 { | |
| 97 v.resize(2); | |
| 98 v[0] = v1; | |
| 99 v[1] = v2; | |
| 100 } | |
| 101 | |
| 102 | |
| 103 void AssignVector(Vector& v, | |
| 104 double v1, | |
| 105 double v2, | |
| 106 double v3) | |
| 107 { | |
| 108 v.resize(3); | |
| 109 v[0] = v1; | |
| 110 v[1] = v2; | |
| 111 v[2] = v3; | |
| 112 } | |
| 113 | |
| 114 | |
| 115 bool IsNear(double x, | |
| 116 double y) | |
| 117 { | |
| 118 // As most input is read as single-precision numbers, we take the | |
| 119 // epsilon machine for float32 into consideration to compare numbers | |
| 120 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon()); | |
| 121 } | |
| 122 | |
| 123 | |
| 124 void NormalizeVector(Vector& u) | |
| 125 { | |
| 126 double norm = boost::numeric::ublas::norm_2(u); | |
| 127 if (!IsCloseToZero(norm)) | |
| 128 { | |
| 129 u = u / norm; | |
| 130 } | |
| 131 } | |
| 132 | |
| 133 | |
| 134 void CrossProduct(Vector& result, | |
| 135 const Vector& u, | |
| 136 const Vector& v) | |
| 137 { | |
| 138 if (u.size() != 3 || | |
| 139 v.size() != 3) | |
| 140 { | |
| 141 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
| 142 } | |
| 143 | |
| 144 result.resize(3); | |
| 145 | |
| 146 result[0] = u[1] * v[2] - u[2] * v[1]; | |
| 147 result[1] = u[2] * v[0] - u[0] * v[2]; | |
| 148 result[2] = u[0] * v[1] - u[1] * v[0]; | |
| 149 } | |
| 150 | |
| 151 | |
| 152 void ProjectPointOntoPlane(Vector& result, | |
| 153 const Vector& point, | |
| 154 const Vector& planeNormal, | |
| 155 const Vector& planeOrigin) | |
| 156 { | |
| 157 double norm = boost::numeric::ublas::norm_2(planeNormal); | |
| 158 if (IsCloseToZero(norm)) | |
| 159 { | |
| 160 // Division by zero | |
| 161 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
| 162 } | |
| 163 | |
| 164 // Make sure the norm of the normal is 1 | |
| 165 Vector n; | |
| 166 n = planeNormal / norm; | |
| 167 | |
| 168 // Algebraic form of line–plane intersection, where the line passes | |
| 169 // through "point" along the direction "normal" (thus, l == n) | |
| 170 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form | |
| 171 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point; | |
| 172 } | |
| 173 | |
| 174 | |
| 175 | |
| 176 bool IsParallelOrOpposite(bool& isOpposite, | |
| 177 const Vector& u, | |
| 178 const Vector& v) | |
| 179 { | |
| 180 // The dot product of the two vectors gives the cosine of the angle | |
| 181 // between the vectors | |
| 182 // https://en.wikipedia.org/wiki/Dot_product | |
| 183 | |
| 184 double normU = boost::numeric::ublas::norm_2(u); | |
| 185 double normV = boost::numeric::ublas::norm_2(v); | |
| 186 | |
| 187 if (IsCloseToZero(normU) || | |
| 188 IsCloseToZero(normV)) | |
| 189 { | |
| 190 return false; | |
| 191 } | |
| 192 | |
| 193 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); | |
| 194 | |
| 195 // The angle must be zero, so the cosine must be almost equal to | |
| 196 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) | |
| 197 | |
| 198 if (IsCloseToZero(cosAngle - 1.0)) | |
| 199 { | |
| 200 isOpposite = false; | |
| 201 return true; | |
| 202 } | |
| 203 else if (IsCloseToZero(fabs(cosAngle) - 1.0)) | |
| 204 { | |
| 205 isOpposite = true; | |
| 206 return true; | |
| 207 } | |
| 208 else | |
| 209 { | |
| 210 return false; | |
| 211 } | |
| 212 } | |
| 213 | |
| 214 | |
| 215 bool IsParallel(const Vector& u, | |
| 216 const Vector& v) | |
| 217 { | |
| 218 bool isOpposite; | |
| 219 return (IsParallelOrOpposite(isOpposite, u, v) && | |
| 220 !isOpposite); | |
| 221 } | |
| 222 | |
| 223 | |
| 224 bool IntersectTwoPlanes(Vector& p, | |
| 225 Vector& direction, | |
| 226 const Vector& origin1, | |
| 227 const Vector& normal1, | |
| 228 const Vector& origin2, | |
| 229 const Vector& normal2) | |
| 230 { | |
| 231 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane | |
| 232 // Intersect Point" of: | |
| 233 // http://geomalgorithms.com/a05-_intersect-1.html | |
| 234 | |
| 235 // The direction of the line of intersection is orthogonal to the | |
| 236 // normal of both planes | |
| 237 CrossProduct(direction, normal1, normal2); | |
| 238 | |
| 239 double norm = boost::numeric::ublas::norm_2(direction); | |
| 240 if (IsCloseToZero(norm)) | |
| 241 { | |
| 242 // The two planes are parallel or coincident | |
| 243 return false; | |
| 244 } | |
| 245 | |
| 246 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); | |
| 247 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); | |
| 248 Vector tmp = d2 * normal1 - d1 * normal2; | |
| 249 | |
| 250 CrossProduct(p, tmp, direction); | |
| 251 p /= norm; | |
| 252 | |
| 253 return true; | |
| 254 } | |
| 255 | |
| 256 | |
| 257 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out) | |
| 258 double& y1, | |
| 259 double& x2, | |
| 260 double& y2, | |
| 261 const double ax, // Two points defining the line (in) | |
| 262 const double ay, | |
| 263 const double bx, | |
| 264 const double by, | |
| 265 const double& xmin, // Coordinates of the rectangle (in) | |
| 266 const double& ymin, | |
| 267 const double& xmax, | |
| 268 const double& ymax) | |
| 269 { | |
| 270 // This is Skala algorithm for rectangles, "A new approach to line | |
| 271 // and line segment clipping in homogeneous coordinates" | |
| 272 // (2005). This is a direct, non-optimized translation of Algorithm | |
| 273 // 2 in the paper. | |
| 274 | |
|
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
275 static const uint8_t tab1[16] = { 255 /* none */, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
276 0, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
277 0, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
278 1, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
279 1, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
280 255 /* na */, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
281 0, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
282 2, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
283 2, |
|
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 255 /* na */, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
286 1, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
287 1, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
288 0, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
289 0, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
290 255 /* none */ }; |
| 0 | 291 |
| 292 | |
|
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
293 static const uint8_t tab2[16] = { 255 /* none */, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
294 3, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
295 1, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
296 3, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
297 2, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
298 255 /* na */, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
299 2, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
300 3, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
301 3, |
|
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 255 /* na */, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
304 2, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
305 3, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
306 1, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
307 3, |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
308 255 /* none */ }; |
| 0 | 309 |
| 310 // Create the coordinates of the rectangle | |
| 311 Vector x[4]; | |
| 312 AssignVector(x[0], xmin, ymin, 1.0); | |
| 313 AssignVector(x[1], xmax, ymin, 1.0); | |
| 314 AssignVector(x[2], xmax, ymax, 1.0); | |
| 315 AssignVector(x[3], xmin, ymax, 1.0); | |
| 316 | |
| 317 // Move to homogoneous coordinates in 2D | |
| 318 Vector p; | |
| 319 | |
| 320 { | |
| 321 Vector a, b; | |
| 322 AssignVector(a, ax, ay, 1.0); | |
| 323 AssignVector(b, bx, by, 1.0); | |
| 324 CrossProduct(p, a, b); | |
| 325 } | |
| 326 | |
| 327 uint8_t c = 0; | |
| 328 | |
| 329 for (unsigned int k = 0; k < 4; k++) | |
| 330 { | |
| 331 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0) | |
| 332 { | |
| 333 c |= (1 << k); | |
| 334 } | |
| 335 } | |
| 336 | |
| 337 assert(c < 16); | |
| 338 | |
| 339 uint8_t i = tab1[c]; | |
| 340 uint8_t j = tab2[c]; | |
| 341 | |
| 342 if (i == 255 || j == 255) | |
| 343 { | |
| 344 return false; // No intersection | |
| 345 } | |
| 346 else | |
| 347 { | |
| 348 Vector a, b, e; | |
| 349 CrossProduct(e, x[i], x[(i + 1) % 4]); | |
| 350 CrossProduct(a, p, e); | |
| 351 CrossProduct(e, x[j], x[(j + 1) % 4]); | |
| 352 CrossProduct(b, p, e); | |
| 353 | |
| 354 // Go back to non-homogeneous coordinates | |
| 355 x1 = a[0] / a[2]; | |
| 356 y1 = a[1] / a[2]; | |
| 357 x2 = b[0] / b[2]; | |
| 358 y2 = b[1] / b[2]; | |
| 359 | |
| 360 return true; | |
| 361 } | |
| 362 } | |
| 32 | 363 |
| 364 | |
| 365 void GetPixelSpacing(double& spacingX, | |
| 366 double& spacingY, | |
|
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
367 const Orthanc::DicomMap& dicom) |
| 32 | 368 { |
| 369 Vector v; | |
| 370 | |
|
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
371 if (ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) |
| 32 | 372 { |
| 373 if (v.size() != 2 || | |
| 374 v[0] <= 0 || | |
| 375 v[1] <= 0) | |
| 376 { | |
| 377 LOG(ERROR) << "Bad value for PixelSpacing tag"; | |
| 378 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat); | |
| 379 } | |
| 380 else | |
| 381 { | |
| 382 spacingX = v[0]; | |
| 383 spacingY = v[1]; | |
| 384 } | |
| 385 } | |
| 386 else | |
| 387 { | |
| 388 // The "PixelSpacing" is of type 1C: It could be absent, use | |
| 389 // default value in such a case | |
| 390 spacingX = 1; | |
| 391 spacingY = 1; | |
| 392 } | |
| 393 } | |
|
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
394 |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
395 |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
396 Matrix CreateRotationMatrixAlongX(double a) |
|
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 // Rotate along X axis (R_x) |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
399 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
400 Matrix r(3, 3); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
401 r(0,0) = 1; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
402 r(0,1) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
403 r(0,2) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
404 r(1,0) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
405 r(1,1) = cos(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
406 r(1,2) = -sin(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
407 r(2,0) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
408 r(2,1) = sin(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
409 r(2,2) = cos(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
410 return r; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
411 } |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
412 |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
413 |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
414 Matrix CreateRotationMatrixAlongY(double a) |
|
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 // Rotate along Y axis (R_y) |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
417 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
418 Matrix r(3, 3); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
419 r(0,0) = cos(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
420 r(0,1) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
421 r(0,2) = sin(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
422 r(1,0) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
423 r(1,1) = 1; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
424 r(1,2) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
425 r(2,0) = -sin(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
426 r(2,1) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
427 r(2,2) = cos(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
428 return r; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
429 } |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
430 |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
431 |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
432 Matrix CreateRotationMatrixAlongZ(double a) |
|
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 // Rotate along Z axis (R_z) |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
435 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
436 Matrix r(3, 3); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
437 r(0,0) = cos(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
438 r(0,1) = -sin(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
439 r(0,2) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
440 r(1,0) = sin(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
441 r(1,1) = cos(a); |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
442 r(1,2) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
443 r(2,0) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
444 r(2,1) = 0; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
445 r(2,2) = 1; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
446 return r; |
|
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
447 } |
| 0 | 448 } |
| 449 } |