Mercurial > hg > orthanc-stone

annotate Framework/Toolbox/GeometryToolbox.cpp @ 1329:8d3e669f01a2 broker

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