Mercurial > hg > orthanc-stone

annotate Framework/Toolbox/GeometryToolbox.cpp @ 984:35ed6812d639 toa2019090602

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Build fix
author Benjamin Golinvaux <bgo@osimis.io>
date Fri, 06 Sep 2019 11:46:51 +0200
parents b70e9be013e4
children 53cc787bd7bc
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
439
b70e9be013e4 preparing for 2019
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 363
diff changeset
5 * Copyright (C) 2017-2019 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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57 bool IsParallelOrOpposite(bool& isOpposite,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58 const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61 // 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
62 // between the vectors
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63 // https://en.wikipedia.org/wiki/Dot_product
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 double normU = boost::numeric::ublas::norm_2(u);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 double normV = boost::numeric::ublas::norm_2(v);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
68 if (LinearAlgebra::IsCloseToZero(normU) ||
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
69 LinearAlgebra::IsCloseToZero(normV))
0
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 return false;
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76 // 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
77 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
79 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0))
0
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 isOpposite = false;
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 }
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
84 else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
86 isOpposite = true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
88 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
89 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
90 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
91 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
92 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
93 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
94
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96 bool IsParallel(const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
99 bool isOpposite;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
100 return (IsParallelOrOpposite(isOpposite, u, v) &&
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 !isOpposite);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 }
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 bool IntersectTwoPlanes(Vector& p,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106 Vector& direction,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
107 const Vector& origin1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
108 const Vector& normal1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 const Vector& origin2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110 const Vector& normal2)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 // Intersect Point" of:
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
114 // http://geomalgorithms.com/a05-_intersect-1.html
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 // The direction of the line of intersection is orthogonal to the
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
117 // normal of both planes
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
118 LinearAlgebra::CrossProduct(direction, normal1, normal2);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 double norm = boost::numeric::ublas::norm_2(direction);
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
121 if (LinearAlgebra::IsCloseToZero(norm))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
122 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123 // The two planes are parallel or coincident
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
124 return false;
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 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129 Vector tmp = d2 * normal1 - d1 * normal2;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
130
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
131 LinearAlgebra::CrossProduct(p, tmp, direction);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
132 p /= 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 return true;
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
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139 double& y1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
140 double& x2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
141 double& y2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142 const double ax, // Two points defining the line (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143 const double ay,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 const double bx,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
145 const double by,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 const double& xmin, // Coordinates of the rectangle (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147 const double& ymin,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
148 const double& xmax,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
149 const double& ymax)
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 // This is Skala algorithm for rectangles, "A new approach to line
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
152 // and line segment clipping in homogeneous coordinates"
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 // (2005). This is a direct, non-optimized translation of Algorithm
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154 // 2 in the paper.
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
155
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
156 static const uint8_t tab1[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
157 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
158 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
159 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
160 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
161 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
162 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
163 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
164 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
165 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
166 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
167 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
168 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
169 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
170 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
171 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
172
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
173
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
174 static const uint8_t tab2[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
175 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
176 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
177 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
178 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
179 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
180 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
181 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
182 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
183 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
184 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
185 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
186 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
187 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
188 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
189 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191 // Create the coordinates of the rectangle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
192 Vector x[4];
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
193 LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
194 LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
195 LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
196 LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0);
0
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 // Move to homogoneous coordinates in 2D
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
199 Vector p;
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 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 Vector a, b;
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
203 LinearAlgebra::AssignVector(a, ax, ay, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
204 LinearAlgebra::AssignVector(b, bx, by, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
205 LinearAlgebra::CrossProduct(p, a, b);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 }
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 uint8_t c = 0;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
210 for (unsigned int k = 0; k < 4; k++)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
211 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 c |= (1 << k);
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 assert(c < 16);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
219
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
220 uint8_t i = tab1[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
221 uint8_t j = tab2[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
222
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
223 if (i == 255 || j == 255)
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 return false; // No intersection
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 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
228 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
229 Vector a, b, e;
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
230 LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
231 LinearAlgebra::CrossProduct(a, p, e);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
232 LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
233 LinearAlgebra::CrossProduct(b, p, e);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
234
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
235 // Go back to non-homogeneous coordinates
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
236 x1 = a[0] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
237 y1 = a[1] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
238 x2 = b[0] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
239 y2 = b[1] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
240
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
241 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
242 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
243 }
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
244
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
245
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
246 void GetPixelSpacing(double& spacingX,
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
247 double& spacingY,
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
248 const Orthanc::DicomMap& dicom)
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
249 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
250 Vector v;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
251
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
252 if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING))
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
253 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
254 if (v.size() != 2 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
255 v[0] <= 0 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
256 v[1] <= 0)
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
257 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
258 LOG(ERROR) << "Bad value for PixelSpacing tag";
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
259 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat);
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
260 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
261 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
262 {
363
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
263 // WARNING: X/Y are swapped (Y comes first)
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
264 spacingX = v[1];
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
265 spacingY = v[0];
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
266 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
267 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
268 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
269 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
270 // The "PixelSpacing" is of type 1C: It could be absent, use
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
271 // default value in such a case
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
272 spacingX = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
273 spacingY = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
274 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
275 }
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
276
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
277
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
278 Matrix CreateRotationMatrixAlongX(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
279 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
280 // Rotate along X axis (R_x)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
281 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
282 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
283 r(0,0) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
284 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
285 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
286 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
287 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
288 r(1,2) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
289 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
290 r(2,1) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
291 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
292 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
293 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
294
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
295
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
296 Matrix CreateRotationMatrixAlongY(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
297 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
298 // Rotate along Y axis (R_y)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
299 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
300 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
301 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
302 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
303 r(0,2) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
304 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
305 r(1,1) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
306 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
307 r(2,0) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
308 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
309 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
310 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
311 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
312
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
313
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
314 Matrix CreateRotationMatrixAlongZ(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
315 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
316 // Rotate along Z axis (R_z)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
317 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
318 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
319 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
320 r(0,1) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
321 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
322 r(1,0) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
323 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
324 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
325 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
326 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
327 r(2,2) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
328 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
329 }
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
330
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
331
165
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
332 Matrix CreateTranslationMatrix(double dx,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
333 double dy,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
334 double dz)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
335 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
336 Matrix m = LinearAlgebra::IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
337 m(0,3) = dx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
338 m(1,3) = dy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
339 m(2,3) = dz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
340 return m;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
341 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
342
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
343
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
344 Matrix CreateScalingMatrix(double sx,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
345 double sy,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
346 double sz)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
347 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
348 Matrix m = LinearAlgebra::IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
349 m(0,0) = sx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
350 m(1,1) = sy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
351 m(2,2) = sz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
352 return m;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
353 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
354
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
355
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
356 bool IntersectPlaneAndSegment(Vector& p,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
357 const Vector& normal,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
358 double d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
359 const Vector& edgeFrom,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
360 const Vector& edgeTo)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
361 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
362 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
363
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
364 // Check for parallel line and plane
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
365 Vector direction = edgeTo - edgeFrom;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
366 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
367
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
368 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
369 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
370 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
371 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
372 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
373 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
374 // Compute intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
375 double t = -(normal[0] * edgeFrom[0] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
376 normal[1] * edgeFrom[1] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
377 normal[2] * edgeFrom[2] + d) / denominator;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
378
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
379 if (t >= 0 && t <= 1)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
380 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
381 // The intersection lies inside edge segment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
382 p = edgeFrom + t * direction;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
383 return true;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
384 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
385 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
386 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
387 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
388 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
389 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
390 }
156
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
391
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
392
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
393 bool IntersectPlaneAndLine(Vector& p,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
394 const Vector& normal,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
395 double d,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
396 const Vector& origin,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
397 const Vector& direction)
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
398 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
399 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
400
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
401 // Check for parallel line and plane
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
402 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
403
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
404 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
405 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
406 return false;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
407 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
408 else
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
409 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
410 // Compute intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
411 double t = -(normal[0] * origin[0] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
412 normal[1] * origin[1] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
413 normal[2] * origin[2] + d) / denominator;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
414
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
415 p = origin + t * direction;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
416 return true;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
417 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
418 }
189
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
419
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
420
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
421 void AlignVectorsWithRotation(Matrix& r,
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
422 const Vector& a,
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
423 const Vector& b)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
424 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
425 // This is Rodrigues' rotation formula:
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
426 // 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
427
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
428 // 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
429 // Vision - 2nd edition" (p. 584)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
430
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
431 if (a.size() != 3 ||
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
432 b.size() != 3)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
433 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
434 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
435 }
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
436
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
437 double aNorm = boost::numeric::ublas::norm_2(a);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
438 double bNorm = boost::numeric::ublas::norm_2(b);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
439
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
440 if (LinearAlgebra::IsCloseToZero(aNorm) ||
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
441 LinearAlgebra::IsCloseToZero(bNorm))
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
442 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
443 LOG(ERROR) << "Vector with zero norm";
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
444 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
445 }
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
446
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
447 Vector aUnit, bUnit;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
448 aUnit = a / aNorm;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
449 bUnit = b / bNorm;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
450
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
451 Vector v;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
452 LinearAlgebra::CrossProduct(v, aUnit, bUnit);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
453
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
454 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
455
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
456 if (LinearAlgebra::IsCloseToZero(1 + cosine))
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
457 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
458 // "a == -b": TODO
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
459 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
460 }
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
461
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
462 Matrix k;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
463 LinearAlgebra::CreateSkewSymmetric(k, v);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
464
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
465 #if 0
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
466 double sine = boost::numeric::ublas::norm_2(v);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
467
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
468 r = (boost::numeric::ublas::identity_matrix<double>(3) +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
469 sine * k +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
470 (1 - cosine) * boost::numeric::ublas::prod(k, k));
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
471 #else
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
472 r = (boost::numeric::ublas::identity_matrix<double>(3) +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
473 k +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
474 boost::numeric::ublas::prod(k, k) / (1 + cosine));
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
475 #endif
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
476 }
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
477 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
478 }