Mercurial > hg > orthanc-stone

annotate OrthancStone/Sources/Toolbox/GeometryToolbox.cpp @ 1622:0f8d6791b403

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
fix build by making the destructor of ViewerViewport public
author Sebastien Jodogne <s.jodogne@gmail.com>
date Tue, 03 Nov 2020 07:04:02 +0100
parents 8563ea5d8ae4
children 9ac2a65d4172
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
1 /**
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
2 * Stone of Orthanc
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
4 * Department, University Hospital of Liege, Belgium
1270
2d8ab34c8c91 upgrade to year 2020
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1013
diff changeset
5 * Copyright (C) 2017-2020 Osimis S.A., Belgium
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
6 *
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
7 * This program is free software: you can redistribute it and/or
1598
8563ea5d8ae4 relicensing some files, cf. osimis bm26 and chu agreement on 2020-05-20
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1596
diff changeset
8 * modify it under the terms of the GNU Lesser General Public License
47
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
9 * as published by the Free Software Foundation, either version 3 of
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
10 * the License, or (at your option) any later version.
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
11 *
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
12 * This program is distributed in the hope that it will be useful, but
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
1598
8563ea5d8ae4 relicensing some files, cf. osimis bm26 and chu agreement on 2020-05-20
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1596
diff changeset
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
8563ea5d8ae4 relicensing some files, cf. osimis bm26 and chu agreement on 2020-05-20
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1596
diff changeset
15 * Lesser General Public License for more details.
1596
4fb8fdf03314 removed annoying whitespace
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1512
diff changeset
16 *
1598
8563ea5d8ae4 relicensing some files, cf. osimis bm26 and chu agreement on 2020-05-20
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1596
diff changeset
17 * You should have received a copy of the GNU Lesser General Public
8563ea5d8ae4 relicensing some files, cf. osimis bm26 and chu agreement on 2020-05-20
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1596
diff changeset
18 * License along with this program. If not, see
8563ea5d8ae4 relicensing some files, cf. osimis bm26 and chu agreement on 2020-05-20
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1596
diff changeset
19 * <http://www.gnu.org/licenses/>.
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
20 **/
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
21
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
22
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
23 #include "GeometryToolbox.h"
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
24
1455
30deba7bc8e2 simplifying include_directories
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1270
diff changeset
25 #include <Logging.h>
30deba7bc8e2 simplifying include_directories
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1270
diff changeset
26 #include <OrthancException.h>
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
28 #include <cassert>
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
30 namespace OrthancStone
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 namespace GeometryToolbox
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 void ProjectPointOntoPlane(Vector& result,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 const Vector& point,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 const Vector& planeNormal,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 const Vector& planeOrigin)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
39 double norm = boost::numeric::ublas::norm_2(planeNormal);
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
40 if (LinearAlgebra::IsCloseToZero(norm))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42 // Division by zero
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46 // Make sure the norm of the normal is 1
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47 Vector n;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48 n = planeNormal / norm;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50 // Algebraic form of line–plane intersection, where the line passes
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 // through "point" along the direction "normal" (thus, l == n)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55
1013
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
56 /*
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
57 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
58 */
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
59 void ProjectPointOntoPlane2(
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
60 double& resultX,
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
61 double& resultY,
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
62 double& resultZ,
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
63 const Vector& point,
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
64 const Vector& planeNormal,
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
65 const Vector& planeOrigin)
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
66 {
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
67 double pointX = point[0];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
68 double pointY = point[1];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
69 double pointZ = point[2];
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70
1013
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
71 double planeNormalX = planeNormal[0];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
72 double planeNormalY = planeNormal[1];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
73 double planeNormalZ = planeNormal[2];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
74
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
75 double planeOriginX = planeOrigin[0];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
76 double planeOriginY = planeOrigin[1];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
77 double planeOriginZ = planeOrigin[2];
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
78
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
79 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
80
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
81 // 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
82 // 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
83 // 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
84
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
85 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
86 {
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
87 double nX = planeNormalX;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
88 double nY = planeNormalY;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
89 double nZ = planeNormalZ;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
90
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
91 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
92
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
93 resultX = prod * nX + pointX;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
94 resultY = prod * nY + pointY;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
95 resultZ = prod * nZ + pointZ;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
96 }
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
97 else
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
98 {
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
99 double norm = sqrt(normSq);
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
100 if (LinearAlgebra::IsCloseToZero(norm))
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
101 {
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
102 // Division by zero
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
103 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
104 }
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
105 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
106 double nX = planeNormalX * invNorm;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
107 double nY = planeNormalY * invNorm;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
108 double nZ = planeNormalZ * invNorm;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
109
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
110 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
111
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
112 resultX = prod * nX + pointX;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
113 resultY = prod * nY + pointY;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
114 resultZ = prod * nZ + pointZ;
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
115 }
53cc787bd7bc - Added an optimized ProjectPoint2 to CoordinateSystem3D. It has *not* replaced
Benjamin Golinvaux <bgo@osimis.io>
parents: 439
diff changeset
116 }
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
117
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
118 bool IsParallelOrOpposite(bool& isOpposite,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119 const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
121 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
122 // The dot product of the two vectors gives the cosine of the angle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123 // between the vectors
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
124 // https://en.wikipedia.org/wiki/Dot_product
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
125
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
126 double normU = boost::numeric::ublas::norm_2(u);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
127 double normV = boost::numeric::ublas::norm_2(v);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
129 if (LinearAlgebra::IsCloseToZero(normU) ||
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
130 LinearAlgebra::IsCloseToZero(normV))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
131 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
132 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
133 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
134
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
135 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
136
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
137 // The angle must be zero, so the cosine must be almost equal to
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
140 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
141 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142 isOpposite = false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 }
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
145 else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147 isOpposite = true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
148 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
149 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
150 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
151 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
152 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
155
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
156
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
157 bool IsParallel(const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
158 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
159 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
160 bool isOpposite;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
161 return (IsParallelOrOpposite(isOpposite, u, v) &&
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
162 !isOpposite);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
163 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
164
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
165
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
166 bool IntersectTwoPlanes(Vector& p,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
167 Vector& direction,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
168 const Vector& origin1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
169 const Vector& normal1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
170 const Vector& origin2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
171 const Vector& normal2)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
172 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
173 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
174 // Intersect Point" of:
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
175 // http://geomalgorithms.com/a05-_intersect-1.html
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
176
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
177 // The direction of the line of intersection is orthogonal to the
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
178 // normal of both planes
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
179 LinearAlgebra::CrossProduct(direction, normal1, normal2);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
180
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
181 double norm = boost::numeric::ublas::norm_2(direction);
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
182 if (LinearAlgebra::IsCloseToZero(norm))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
183 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
184 // The two planes are parallel or coincident
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
185 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
186 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
187
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
188 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
189 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190 Vector tmp = d2 * normal1 - d1 * normal2;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
192 LinearAlgebra::CrossProduct(p, tmp, direction);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
193 p /= norm;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
194
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
195 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
196 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
197
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
198
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
199 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
200 double& y1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
201 double& x2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 double& y2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
203 const double ax, // Two points defining the line (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
204 const double ay,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
205 const double bx,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 const double by,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
207 const double& xmin, // Coordinates of the rectangle (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
208 const double& ymin,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209 const double& xmax,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
210 const double& ymax)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
211 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 // This is Skala algorithm for rectangles, "A new approach to line
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 // and line segment clipping in homogeneous coordinates"
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 // (2005). This is a direct, non-optimized translation of Algorithm
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
215 // 2 in the paper.
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
216
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
217 static const uint8_t tab1[16] = { 255 /* none */,
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 0,
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 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
222 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
223 0,
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 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
226 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
227 255 /* na */,
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 1,
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 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
232 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
233
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
234
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
235 static const uint8_t tab2[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
236 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
237 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
238 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
239 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
240 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
241 2,
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 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
244 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
245 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
246 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
247 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
248 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
249 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
250 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
251
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
252 // Create the coordinates of the rectangle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
253 Vector x[4];
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
254 LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
255 LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
256 LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
257 LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
258
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
259 // Move to homogoneous coordinates in 2D
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
260 Vector p;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
261
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
262 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
263 Vector a, b;
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
264 LinearAlgebra::AssignVector(a, ax, ay, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
265 LinearAlgebra::AssignVector(b, bx, by, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
266 LinearAlgebra::CrossProduct(p, a, b);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
267 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
268
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
269 uint8_t c = 0;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
270
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
271 for (unsigned int k = 0; k < 4; k++)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
272 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
273 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
274 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
275 c |= (1 << k);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
276 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
277 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
278
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
279 assert(c < 16);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
280
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
281 uint8_t i = tab1[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
282 uint8_t j = tab2[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
283
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
284 if (i == 255 || j == 255)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
285 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
286 return false; // No intersection
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
287 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
288 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
289 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
290 Vector a, b, e;
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
291 LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
292 LinearAlgebra::CrossProduct(a, p, e);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
293 LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
294 LinearAlgebra::CrossProduct(b, p, e);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
295
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
296 // Go back to non-homogeneous coordinates
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
297 x1 = a[0] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
298 y1 = a[1] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
299 x2 = b[0] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
300 y2 = b[1] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
301
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
302 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
303 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
304 }
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
305
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
306
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
307 void GetPixelSpacing(double& spacingX,
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
308 double& spacingY,
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
309 const Orthanc::DicomMap& dicom)
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
310 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
311 Vector v;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
312
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
313 if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING))
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
314 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
315 if (v.size() != 2 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
316 v[0] <= 0 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
317 v[1] <= 0)
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
318 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
319 LOG(ERROR) << "Bad value for PixelSpacing tag";
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
320 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat);
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
321 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
322 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
323 {
363
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
324 // WARNING: X/Y are swapped (Y comes first)
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
325 spacingX = v[1];
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
326 spacingY = v[0];
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
327 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
328 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
329 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
330 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
331 // The "PixelSpacing" is of type 1C: It could be absent, use
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
332 // default value in such a case
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
333 spacingX = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
334 spacingY = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
335 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
336 }
140
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
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
339 Matrix CreateRotationMatrixAlongX(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
340 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
341 // Rotate along X axis (R_x)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
342 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
343 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
344 r(0,0) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
345 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
346 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
347 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
348 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
349 r(1,2) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
350 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
351 r(2,1) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
352 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
353 return r;
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
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
357 Matrix CreateRotationMatrixAlongY(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
358 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
359 // Rotate along Y axis (R_y)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
360 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
361 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
362 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
363 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
364 r(0,2) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
365 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
366 r(1,1) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
367 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
368 r(2,0) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
369 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
370 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
371 return r;
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
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
375 Matrix CreateRotationMatrixAlongZ(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
376 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
377 // Rotate along Z axis (R_z)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
378 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
379 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
380 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
381 r(0,1) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
382 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
383 r(1,0) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
384 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
385 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
386 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
387 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
388 r(2,2) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
389 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
390 }
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
391
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
392
165
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
393 Matrix CreateTranslationMatrix(double dx,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
394 double dy,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
395 double dz)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
396 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
397 Matrix m = LinearAlgebra::IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
398 m(0,3) = dx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
399 m(1,3) = dy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
400 m(2,3) = dz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
401 return m;
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
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
405 Matrix CreateScalingMatrix(double sx,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
406 double sy,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
407 double sz)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
408 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
409 Matrix m = LinearAlgebra::IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
410 m(0,0) = sx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
411 m(1,1) = sy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
412 m(2,2) = sz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
413 return m;
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
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
416
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
417 bool IntersectPlaneAndSegment(Vector& p,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
418 const Vector& normal,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
419 double d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
420 const Vector& edgeFrom,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
421 const Vector& edgeTo)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
422 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
423 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
424
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
425 // Check for parallel line and plane
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
426 Vector direction = edgeTo - edgeFrom;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
427 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
428
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
429 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
430 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
431 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
432 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
433 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
434 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
435 // Compute intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
436 double t = -(normal[0] * edgeFrom[0] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
437 normal[1] * edgeFrom[1] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
438 normal[2] * edgeFrom[2] + d) / denominator;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
439
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
440 if (t >= 0 && t <= 1)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
441 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
442 // The intersection lies inside edge segment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
443 p = edgeFrom + t * direction;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
444 return true;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
445 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
446 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
447 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
448 return false;
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 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
451 }
156
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
452
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
453
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
454 bool IntersectPlaneAndLine(Vector& p,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
455 const Vector& normal,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
456 double d,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
457 const Vector& origin,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
458 const Vector& direction)
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
459 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
460 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
461
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
462 // Check for parallel line and plane
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
463 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
464
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
465 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
466 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
467 return false;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
468 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
469 else
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
470 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
471 // Compute intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
472 double t = -(normal[0] * origin[0] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
473 normal[1] * origin[1] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
474 normal[2] * origin[2] + d) / denominator;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
475
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
476 p = origin + t * direction;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
477 return true;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
478 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
479 }
189
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
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
482 void AlignVectorsWithRotation(Matrix& r,
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
483 const Vector& a,
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
484 const Vector& b)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
485 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
486 // This is Rodrigues' rotation formula:
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
487 // 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
488
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
489 // 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
490 // Vision - 2nd edition" (p. 584)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
491
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
492 if (a.size() != 3 ||
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
493 b.size() != 3)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
494 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
495 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
498 double aNorm = boost::numeric::ublas::norm_2(a);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
499 double bNorm = boost::numeric::ublas::norm_2(b);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
500
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
501 if (LinearAlgebra::IsCloseToZero(aNorm) ||
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
502 LinearAlgebra::IsCloseToZero(bNorm))
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
503 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
504 LOG(ERROR) << "Vector with zero norm";
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
505 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
508 Vector aUnit, bUnit;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
509 aUnit = a / aNorm;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
510 bUnit = b / bNorm;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
511
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
512 Vector v;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
513 LinearAlgebra::CrossProduct(v, aUnit, bUnit);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
514
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
515 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
516
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
517 if (LinearAlgebra::IsCloseToZero(1 + cosine))
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
518 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
519 // "a == -b": TODO
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
520 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
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
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
523 Matrix k;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
524 LinearAlgebra::CreateSkewSymmetric(k, v);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
525
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
526 #if 0
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
527 double sine = boost::numeric::ublas::norm_2(v);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
528
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
529 r = (boost::numeric::ublas::identity_matrix<double>(3) +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
530 sine * k +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
531 (1 - cosine) * boost::numeric::ublas::prod(k, k));
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
532 #else
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
533 r = (boost::numeric::ublas::identity_matrix<double>(3) +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
534 k +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
535 boost::numeric::ublas::prod(k, k) / (1 + cosine));
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
536 #endif
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
537 }
1477
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
538
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
539
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
540 void ComputeNormalFromCosines(Vector& normal,
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
541 const Vector& cosines)
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
542 {
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
543 if (cosines.size() != 6)
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
544 {
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
545 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
546 }
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
547 else
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
548 {
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
549 normal.resize(3);
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
550 normal[0] = cosines[1] * cosines[5] - cosines[2] * cosines[4];
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
551 normal[1] = cosines[2] * cosines[3] - cosines[0] * cosines[5];
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
552 normal[2] = cosines[0] * cosines[4] - cosines[1] * cosines[3];
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
553 }
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
554 }
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
555
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
556
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
557 bool ComputeNormal(Vector& normal,
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
558 const Orthanc::DicomMap& dicom)
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
559 {
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
560 Vector cosines;
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
561 if (LinearAlgebra::ParseVector(cosines, dicom, Orthanc::DICOM_TAG_IMAGE_ORIENTATION_PATIENT) &&
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
562 cosines.size() == 6)
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
563 {
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
564 ComputeNormalFromCosines(normal, cosines);
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
565 return true;
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
566 }
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
567 else
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
568 {
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
569 return false;
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
570 }
5732edec7cbd sorting frames in 3D
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 1455
diff changeset
571 }
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
572 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
573 }