annotate Framework/Toolbox/OrientedBoundingBox.cpp @ 194:7a031ac16b2d wasm

rename Enumerations.h to StoneEnumerations.h to avoid clashes with Orthanc
author Sebastien Jodogne <s.jodogne@gmail.com>
date Tue, 20 Mar 2018 14:46:58 +0100
parents 0a73d76333db
children e9c7a78a3e77
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
1 /**
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
2 * Stone of Orthanc
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
4 * Department, University Hospital of Liege, Belgium
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
5 * Copyright (C) 2017-2018 Osimis S.A., Belgium
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
6 *
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
7 * This program is free software: you can redistribute it and/or
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
8 * modify it under the terms of the GNU Affero General Public License
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
9 * as published by the Free Software Foundation, either version 3 of
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
10 * the License, or (at your option) any later version.
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
11 *
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
12 * This program is distributed in the hope that it will be useful, but
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
15 * Affero General Public License for more details.
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
16 *
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
17 * You should have received a copy of the GNU Affero General Public License
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
18 * along with this program. If not, see <http://www.gnu.org/licenses/>.
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
19 **/
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
20
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
21
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
22 #include "OrientedBoundingBox.h"
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
23
159
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
24 #include "GeometryToolbox.h"
0a73d76333db populating LinearAlgebra
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
25
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
26 #include <Core/OrthancException.h>
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28 #include <cassert>
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
30 namespace OrthancStone
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 OrientedBoundingBox::OrientedBoundingBox(const ImageBuffer3D& image)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 unsigned int n = image.GetDepth();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 if (n < 1)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 throw Orthanc::OrthancException(Orthanc::ErrorCode_IncompatibleImageSize);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
39
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
40 const CoordinateSystem3D& geometry = image.GetAxialGeometry();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41 Vector dim = image.GetVoxelDimensions(VolumeProjection_Axial);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43 u_ = geometry.GetAxisX();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44 v_ = geometry.GetAxisY();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45 w_ = geometry.GetNormal();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47 hu_ = static_cast<double>(image.GetWidth() * dim[0] / 2.0);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48 hv_ = static_cast<double>(image.GetHeight() * dim[1] / 2.0);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49 hw_ = static_cast<double>(image.GetDepth() * dim[2] / 2.0);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 c_ = (geometry.GetOrigin() +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 (hu_ - dim[0] / 2.0) * u_ +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53 (hv_ - dim[1] / 2.0) * v_ +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54 (hw_ - dim[2] / 2.0) * w_);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58 bool OrientedBoundingBox::HasIntersectionWithPlane(std::vector<Vector>& points,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 const Vector& normal,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60 double d) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62 assert(normal.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64 double r = (hu_ * fabs(boost::numeric::ublas::inner_prod(normal, u_)) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 hv_ * fabs(boost::numeric::ublas::inner_prod(normal, v_)) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 hw_ * fabs(boost::numeric::ublas::inner_prod(normal, w_)));
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
68 double s = boost::numeric::ublas::inner_prod(normal, c_) + d;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
69
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70 if (fabs(s) >= r)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 // No intersection, or intersection is reduced to a single point
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73 return false;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75 else
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77 Vector p;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
79 // Loop over all the 12 edges (segments) of the oriented
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80 // bounding box, and check whether they intersect the plane
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
81
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
82 // X-aligned edges
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
83 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
84 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
85 c_ - u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
86 c_ + u_ * hu_ - v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
88 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
89 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
90
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
91 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
92 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
93 c_ - u_ * hu_ + v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
94 c_ + u_ * hu_ + v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
99 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
100 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
101 c_ - u_ * hu_ - v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
102 c_ + u_ * hu_ - v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
105 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
107 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
108 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
109 c_ - u_ * hu_ + v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
110 c_ + u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
114
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
115 // Y-aligned edges
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
116 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
117 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
118 c_ - u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
119 c_ - u_ * hu_ + v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
121 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
122 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
124 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
125 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
126 c_ + u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
127 c_ + u_ * hu_ + v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
130 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
131
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
132 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
133 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
134 c_ - u_ * hu_ - v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
135 c_ - u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
136 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
137 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
140 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
141 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
142 c_ + u_ * hu_ - v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
143 c_ + u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
145 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
148 // Z-aligned edges
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
149 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
150 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
151 c_ - u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
152 c_ - u_ * hu_ - v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
155 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
156
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
157 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
158 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
159 c_ + u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
160 c_ + u_ * hu_ - v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
161 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
162 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
163 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
164
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
165 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
166 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
167 c_ - u_ * hu_ + v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
168 c_ - u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
169 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
170 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
171 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
172
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
173 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
174 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
175 c_ + u_ * hu_ + v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
176 c_ + u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
177 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
178 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
179 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
180
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
181 return true;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
182 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
183 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
184
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
185
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
186 bool OrientedBoundingBox::HasIntersection(std::vector<Vector>& points,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
187 const CoordinateSystem3D& plane) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
188 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
189 // From the vector equation of a 3D plane (specified by origin
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190 // and normal), to the general equation of a 3D plane (which
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191 // looses information about the origin of the coordinate system)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
192 const Vector& normal = plane.GetNormal();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
193 const Vector& origin = plane.GetOrigin();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
194 double d = -(normal[0] * origin[0] + normal[1] * origin[1] + normal[2] * origin[2]);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
195
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
196 return HasIntersectionWithPlane(points, normal, d);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
197 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
198
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
199
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
200 bool OrientedBoundingBox::Contains(const Vector& p) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
201 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 assert(p.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
203
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
204 const Vector q = p - c_;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
205
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 return (fabs(boost::numeric::ublas::inner_prod(q, u_)) <= hu_ &&
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
207 fabs(boost::numeric::ublas::inner_prod(q, v_)) <= hv_ &&
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
208 fabs(boost::numeric::ublas::inner_prod(q, w_)) <= hw_);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
210
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
211
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 void OrientedBoundingBox::FromInternalCoordinates(Vector& target,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 double x,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 double y,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
215 double z) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
216 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
217 target = (c_ +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
218 u_ * 2.0 * hu_ * (x - 0.5) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
219 v_ * 2.0 * hv_ * (y - 0.5) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
220 w_ * 2.0 * hw_ * (z - 0.5));
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
221 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
222
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
223
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
224 void OrientedBoundingBox::FromInternalCoordinates(Vector& target,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
225 const Vector& source) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
226 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
227 assert(source.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
228 FromInternalCoordinates(target, source[0], source[1], source[2]);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
229 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
230
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
231
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
232 void OrientedBoundingBox::ToInternalCoordinates(Vector& target,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
233 const Vector& source) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
234 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
235 assert(source.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
236 const Vector q = source - c_;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
237
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
238 double x = boost::numeric::ublas::inner_prod(q, u_) / (2.0 * hu_) + 0.5;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
239 double y = boost::numeric::ublas::inner_prod(q, v_) / (2.0 * hv_) + 0.5;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
240 double z = boost::numeric::ublas::inner_prod(q, w_) / (2.0 * hw_) + 0.5;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
241
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
242 LinearAlgebra::AssignVector(target, x, y, z);
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
243 }
146
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
244
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
245
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
246 bool OrientedBoundingBox::ComputeExtent(Extent2D& extent,
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
247 const CoordinateSystem3D& plane) const
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
248 {
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
249 extent.Reset();
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
250
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
251 std::vector<Vector> points;
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
252 if (HasIntersection(points, plane))
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
253 {
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
254 for (size_t i = 0; i < points.size(); i++)
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
255 {
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
256 double x, y;
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
257 plane.ProjectPoint(x, y, points[i]);
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
258 extent.AddPoint(x, y);
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
259 }
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
260
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
261 return true;
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
262 }
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
263 else
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
264 {
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
265 return false;
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
266 }
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
267 }
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
268 }