Mercurial > hg > orthanc-stone

annotate Framework/Toolbox/GeometryToolbox.cpp @ 157:2309e8d86efe wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
IntersectPlaneAndLine
author Sebastien Jodogne <s.jodogne@gmail.com>
date Fri, 09 Feb 2018 16:00:29 +0100
parents 441cfe8e7440
children a053ca7fa5c6
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
135
e2fe9352f240 upgrade to year 2018
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 118
diff changeset
5 * Copyright (C) 2017-2018 Osimis S.A., Belgium
0
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
113
2eca030792aa using the Orthanc Framework
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 47
diff changeset
24 #include <Core/Logging.h>
2eca030792aa using the Orthanc Framework
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 47
diff changeset
25 #include <Core/OrthancException.h>
2eca030792aa using the Orthanc Framework
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 47
diff changeset
26 #include <Core/Toolbox.h>
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28 #include <stdio.h>
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29 #include <boost/lexical_cast.hpp>
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 OrthancStone
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 namespace GeometryToolbox
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 void Print(const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 for (size_t i = 0; i < v.size(); i++)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 {
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
39 printf("%g\n", v[i]);
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
40 //printf("%8.2f\n", v[i]);
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 printf("\n");
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
150
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
46 void Print(const Matrix& m)
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
47 {
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
48 for (size_t i = 0; i < m.size1(); i++)
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
49 {
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
50 for (size_t j = 0; j < m.size2(); j++)
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
51 {
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
52 printf("%g ", m(i,j));
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
53 //printf("%8.2f ", m(i,j));
150
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
54 }
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
55 printf("\n");
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
56 }
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
57 printf("\n");
150
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
58 }
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
59
62670cc2bb50 print matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
60
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61 bool ParseVector(Vector& target,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62 const std::string& value)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64 std::vector<std::string> items;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 Orthanc::Toolbox::TokenizeString(items, value, '\\');
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67 target.resize(items.size());
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
68
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
69 for (size_t i = 0; i < items.size(); i++)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71 try
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i]));
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75 catch (boost::bad_lexical_cast&)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77 target.clear();
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
79 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
81
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
82 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
83 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
84
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
86 bool ParseVector(Vector& target,
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
87 const Orthanc::DicomMap& dataset,
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
88 const Orthanc::DicomTag& tag)
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
89 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
90 std::string value;
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
91 return (dataset.CopyToString(value, tag, false) &&
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
92 ParseVector(target, value));
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
93 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
94
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
95
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96 void AssignVector(Vector& v,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 double v1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98 double v2)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
99 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
100 v.resize(2);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 v[0] = v1;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 v[1] = v2;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
105
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106 void AssignVector(Vector& v,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
107 double v1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
108 double v2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 double v3)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 v.resize(3);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 v[0] = v1;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 v[1] = v2;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
114 v[2] = v3;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
115 }
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
118 bool IsNear(double x,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119 double y)
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 // As most input is read as single-precision numbers, we take the
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
122 // epsilon machine for float32 into consideration to compare numbers
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon());
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
126
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
127 void NormalizeVector(Vector& u)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129 double norm = boost::numeric::ublas::norm_2(u);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
130 if (!IsCloseToZero(norm))
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 u = u / norm;
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
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 void CrossProduct(Vector& result,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139 const Vector& v)
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 if (u.size() != 3 ||
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142 v.size() != 3)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147 result.resize(3);
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 result[0] = u[1] * v[2] - u[2] * v[1];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
150 result[1] = u[2] * v[0] - u[0] * v[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
151 result[2] = u[0] * v[1] - u[1] * v[0];
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 void ProjectPointOntoPlane(Vector& result,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
156 const Vector& point,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
157 const Vector& planeNormal,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
158 const Vector& planeOrigin)
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 double norm = boost::numeric::ublas::norm_2(planeNormal);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
161 if (IsCloseToZero(norm))
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 // Division by zero
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
164 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
167 // Make sure the norm of the normal is 1
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
168 Vector n;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
169 n = planeNormal / norm;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
170
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
171 // Algebraic form of line–plane intersection, where the line passes
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
172 // through "point" along the direction "normal" (thus, l == n)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
173 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
174 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point;
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
177
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
178
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
179 bool IsParallelOrOpposite(bool& isOpposite,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
180 const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
181 const Vector& v)
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 dot product of the two vectors gives the cosine of the angle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
184 // between the vectors
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
185 // https://en.wikipedia.org/wiki/Dot_product
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 normU = boost::numeric::ublas::norm_2(u);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
188 double normV = boost::numeric::ublas::norm_2(v);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
189
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190 if (IsCloseToZero(normU) ||
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191 IsCloseToZero(normV))
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
192 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
193 return false;
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
196 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV);
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 // 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
199 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
200
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
201 if (IsCloseToZero(cosAngle - 1.0))
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
203 isOpposite = false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
204 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
205 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 else if (IsCloseToZero(fabs(cosAngle) - 1.0))
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
207 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
208 isOpposite = true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209 return true;
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 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
215 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
216
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
217
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
218 bool IsParallel(const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
219 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
220 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
221 bool isOpposite;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
222 return (IsParallelOrOpposite(isOpposite, u, v) &&
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
223 !isOpposite);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
224 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
225
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
226
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
227 bool IntersectTwoPlanes(Vector& p,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
228 Vector& direction,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
229 const Vector& origin1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
230 const Vector& normal1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
231 const Vector& origin2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
232 const Vector& normal2)
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 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
235 // Intersect Point" of:
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
236 // http://geomalgorithms.com/a05-_intersect-1.html
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
237
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
238 // The direction of the line of intersection is orthogonal to the
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
239 // normal of both planes
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
240 CrossProduct(direction, normal1, normal2);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
241
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
242 double norm = boost::numeric::ublas::norm_2(direction);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
243 if (IsCloseToZero(norm))
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
244 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
245 // The two planes are parallel or coincident
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
246 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
247 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
248
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
249 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
250 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
251 Vector tmp = d2 * normal1 - d1 * normal2;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
252
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
253 CrossProduct(p, tmp, direction);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
254 p /= norm;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
255
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
256 return true;
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
259
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
260 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
261 double& y1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
262 double& x2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
263 double& y2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
264 const double ax, // Two points defining the line (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
265 const double ay,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
266 const double bx,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
267 const double by,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
268 const double& xmin, // Coordinates of the rectangle (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
269 const double& ymin,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
270 const double& xmax,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
271 const double& ymax)
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 // This is Skala algorithm for rectangles, "A new approach to line
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
274 // and line segment clipping in homogeneous coordinates"
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
275 // (2005). This is a direct, non-optimized translation of Algorithm
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
276 // 2 in the paper.
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
277
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
278 static const uint8_t tab1[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
279 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
280 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
281 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
282 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
283 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
284 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
285 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
286 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
287 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
288 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
289 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
290 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
291 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
292 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
293 255 /* none */ };
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
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
296 static const uint8_t tab2[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
297 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
298 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
299 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
300 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
301 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
302 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
303 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
304 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
305 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
306 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
307 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
308 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
309 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
310 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
311 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
312
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
313 // Create the coordinates of the rectangle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
314 Vector x[4];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
315 AssignVector(x[0], xmin, ymin, 1.0);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
316 AssignVector(x[1], xmax, ymin, 1.0);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
317 AssignVector(x[2], xmax, ymax, 1.0);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
318 AssignVector(x[3], xmin, ymax, 1.0);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
319
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
320 // Move to homogoneous coordinates in 2D
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
321 Vector p;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
322
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
323 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
324 Vector a, b;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
325 AssignVector(a, ax, ay, 1.0);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
326 AssignVector(b, bx, by, 1.0);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
327 CrossProduct(p, a, b);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
328 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
329
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
330 uint8_t c = 0;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
331
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
332 for (unsigned int k = 0; k < 4; k++)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
333 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
334 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
335 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
336 c |= (1 << k);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
337 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
338 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
339
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
340 assert(c < 16);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
341
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
342 uint8_t i = tab1[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
343 uint8_t j = tab2[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
344
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
345 if (i == 255 || j == 255)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
346 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
347 return false; // No intersection
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
348 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
349 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
350 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
351 Vector a, b, e;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
352 CrossProduct(e, x[i], x[(i + 1) % 4]);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
353 CrossProduct(a, p, e);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
354 CrossProduct(e, x[j], x[(j + 1) % 4]);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
355 CrossProduct(b, p, e);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
356
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
357 // Go back to non-homogeneous coordinates
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
358 x1 = a[0] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
359 y1 = a[1] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
360 x2 = b[0] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
361 y2 = b[1] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
362
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
363 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
364 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
365 }
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
366
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
367
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
368 void GetPixelSpacing(double& spacingX,
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
369 double& spacingY,
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
370 const Orthanc::DicomMap& dicom)
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
371 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
372 Vector v;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
373
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
374 if (ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING))
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
375 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
376 if (v.size() != 2 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
377 v[0] <= 0 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
378 v[1] <= 0)
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
379 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
380 LOG(ERROR) << "Bad value for PixelSpacing tag";
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
381 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat);
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
382 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
383 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
384 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
385 spacingX = v[0];
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
386 spacingY = v[1];
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
387 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
388 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
389 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
390 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
391 // The "PixelSpacing" is of type 1C: It could be absent, use
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
392 // default value in such a case
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
393 spacingX = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
394 spacingY = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
395 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
396 }
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
397
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
398
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
399 Matrix CreateRotationMatrixAlongX(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
400 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
401 // Rotate along X axis (R_x)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
402 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
403 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
404 r(0,0) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
405 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
406 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
407 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
408 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
409 r(1,2) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
410 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
411 r(2,1) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
412 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
413 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
414 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
415
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
416
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
417 Matrix CreateRotationMatrixAlongY(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
418 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
419 // Rotate along Y axis (R_y)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
420 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
421 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
422 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
423 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
424 r(0,2) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
425 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
426 r(1,1) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
427 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
428 r(2,0) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
429 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
430 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
431 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
432 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
433
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
434
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
435 Matrix CreateRotationMatrixAlongZ(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
436 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
437 // Rotate along Z axis (R_z)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
438 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
439 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
440 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
441 r(0,1) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
442 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
443 r(1,0) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
444 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
445 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
446 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
447 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
448 r(2,2) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
449 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
450 }
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
451
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
452
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
453 bool IntersectPlaneAndSegment(Vector& p,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
454 const Vector& normal,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
455 double d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
456 const Vector& edgeFrom,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
457 const Vector& edgeTo)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
458 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
459 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
460
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
461 // Check for parallel line and plane
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
462 Vector direction = edgeTo - edgeFrom;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
463 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
464
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
465 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
466 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
467 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
468 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
469 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
470 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
471 // Compute intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
472 double t = -(normal[0] * edgeFrom[0] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
473 normal[1] * edgeFrom[1] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
474 normal[2] * edgeFrom[2] + d) / denominator;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
475
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
476 if (t >= 0 && t <= 1)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
477 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
478 // The intersection lies inside edge segment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
479 p = edgeFrom + t * direction;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
480 return true;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
481 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
482 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
483 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
484 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
485 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
486 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
487 }
156
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
488
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
489
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
490 bool IntersectPlaneAndLine(Vector& p,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
491 const Vector& normal,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
492 double d,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
493 const Vector& origin,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
494 const Vector& direction)
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
495 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
496 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
497
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
498 // Check for parallel line and plane
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
499 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
500
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
501 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
502 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
503 return false;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
504 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
505 else
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
506 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
507 // Compute intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
508 double t = -(normal[0] * origin[0] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
509 normal[1] * origin[1] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
510 normal[2] * origin[2] + d) / denominator;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
511
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
512 p = origin + t * direction;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
513 return true;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
514 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
515 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
516
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
517
156
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
518 void FillMatrix(Matrix& target,
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
519 size_t rows,
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
520 size_t columns,
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
521 const double values[])
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
522 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
523 target.resize(rows, columns);
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
524
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
525 size_t index = 0;
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
526
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
527 for (size_t y = 0; y < rows; y++)
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
528 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
529 for (size_t x = 0; x < columns; x++, index++)
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
530 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
531 target(y, x) = values[index];
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
532 }
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
533 }
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
534 }
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
535
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
536
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
537 void FillVector(Vector& target,
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
538 size_t size,
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
539 const double values[])
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
540 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
541 target.resize(size);
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
542
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
543 for (size_t i = 0; i < size; i++)
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
544 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
545 target[i] = values[i];
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
546 }
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
547 }
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
548
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
549
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
550 void Convert(Matrix& target,
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
551 const Vector& source)
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
552 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
553 const size_t n = source.size();
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
554
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
555 target.resize(n, 1);
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
556
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
557 for (size_t i = 0; i < n; i++)
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
558 {
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
559 target(i, 0) = source[i];
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
560 }
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
561 }
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
562 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
563 }