annotate Framework/Toolbox/OrientedBoundingBox.cpp @ 158:a053ca7fa5c6 wasm

LinearAlgebra toolbox
author Sebastien Jodogne <s.jodogne@gmail.com>
date Wed, 14 Feb 2018 08:58:31 +0100
parents c5044bbfc303
children 0a73d76333db
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
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
24 #include <Core/OrthancException.h>
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
25
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
26 #include <cassert>
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
27
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28 namespace OrthancStone
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
30 OrientedBoundingBox::OrientedBoundingBox(const ImageBuffer3D& image)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 unsigned int n = image.GetDepth();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 if (n < 1)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 throw Orthanc::OrthancException(Orthanc::ErrorCode_IncompatibleImageSize);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 const CoordinateSystem3D& geometry = image.GetAxialGeometry();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
39 Vector dim = image.GetVoxelDimensions(VolumeProjection_Axial);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
40
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41 u_ = geometry.GetAxisX();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42 v_ = geometry.GetAxisY();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43 w_ = geometry.GetNormal();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45 hu_ = static_cast<double>(image.GetWidth() * dim[0] / 2.0);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46 hv_ = static_cast<double>(image.GetHeight() * dim[1] / 2.0);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47 hw_ = static_cast<double>(image.GetDepth() * dim[2] / 2.0);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49 c_ = (geometry.GetOrigin() +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50 (hu_ - dim[0] / 2.0) * u_ +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 (hv_ - dim[1] / 2.0) * v_ +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 (hw_ - dim[2] / 2.0) * w_);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56 bool OrientedBoundingBox::HasIntersectionWithPlane(std::vector<Vector>& points,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57 const Vector& normal,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58 double d) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60 assert(normal.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62 double r = (hu_ * fabs(boost::numeric::ublas::inner_prod(normal, u_)) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63 hv_ * fabs(boost::numeric::ublas::inner_prod(normal, v_)) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64 hw_ * fabs(boost::numeric::ublas::inner_prod(normal, w_)));
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 double s = boost::numeric::ublas::inner_prod(normal, c_) + d;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
68 if (fabs(s) >= r)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
69 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70 // No intersection, or intersection is reduced to a single point
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71 return false;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73 else
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75 Vector p;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77 // Loop over all the 12 edges (segments) of the oriented
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78 // bounding box, and check whether they intersect the plane
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
79
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80 // X-aligned edges
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
81 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
82 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
83 c_ - u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
84 c_ + u_ * hu_ - v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
86 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
88
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
89 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
90 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
91 c_ - u_ * hu_ + v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
92 c_ + u_ * hu_ + v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
93 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
94 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
97 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
98 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
99 c_ - u_ * hu_ - v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
100 c_ + u_ * hu_ - v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
105 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
106 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
107 c_ - u_ * hu_ + v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
108 c_ + u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 // Y-aligned edges
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
114 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
115 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
116 c_ - u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
117 c_ - u_ * hu_ + v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
118 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
121
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
122 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
123 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
124 c_ + u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
125 c_ + u_ * hu_ + v_ * hv_ - w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
126 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
127 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
130 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
131 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
132 c_ - u_ * hu_ - v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
133 c_ - u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
134 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
135 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
136 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
137
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
138 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
139 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
140 c_ + u_ * hu_ - v_ * hv_ + w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
141 c_ + u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
145
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 // Z-aligned edges
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
147 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
148 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
149 c_ - u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
150 c_ - u_ * hu_ - v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
151 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
152 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
155 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
156 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
157 c_ + u_ * hu_ - v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
158 c_ + u_ * hu_ - v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
159 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
160 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
161 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
162
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
163 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
164 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
165 c_ - u_ * hu_ + v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
166 c_ - u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
167 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
168 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
169 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
170
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
171 if (GeometryToolbox::IntersectPlaneAndSegment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
172 (p, normal, d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
173 c_ + u_ * hu_ + v_ * hv_ - w_ * hw_,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 146
diff changeset
174 c_ + u_ * hu_ + v_ * hv_ + w_ * hw_))
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
175 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
176 points.push_back(p);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
177 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
178
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
179 return true;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
180 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
181 }
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 bool OrientedBoundingBox::HasIntersection(std::vector<Vector>& points,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
185 const CoordinateSystem3D& plane) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
186 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
187 // From the vector equation of a 3D plane (specified by origin
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
188 // and normal), to the general equation of a 3D plane (which
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
189 // looses information about the origin of the coordinate system)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190 const Vector& normal = plane.GetNormal();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191 const Vector& origin = plane.GetOrigin();
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
192 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
193
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
194 return HasIntersectionWithPlane(points, normal, d);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
195 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
196
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
197
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
198 bool OrientedBoundingBox::Contains(const Vector& p) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
199 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
200 assert(p.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
201
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 const Vector q = p - c_;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
203
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
204 return (fabs(boost::numeric::ublas::inner_prod(q, u_)) <= hu_ &&
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
205 fabs(boost::numeric::ublas::inner_prod(q, v_)) <= hv_ &&
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 fabs(boost::numeric::ublas::inner_prod(q, w_)) <= hw_);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
207 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
208
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
210 void OrientedBoundingBox::FromInternalCoordinates(Vector& target,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
211 double x,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 double y,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 double z) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
215 target = (c_ +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
216 u_ * 2.0 * hu_ * (x - 0.5) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
217 v_ * 2.0 * hv_ * (y - 0.5) +
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
218 w_ * 2.0 * hw_ * (z - 0.5));
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
219 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
220
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
221
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
222 void OrientedBoundingBox::FromInternalCoordinates(Vector& target,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
223 const Vector& source) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
224 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
225 assert(source.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
226 FromInternalCoordinates(target, source[0], source[1], source[2]);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
227 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
228
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
229
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
230 void OrientedBoundingBox::ToInternalCoordinates(Vector& target,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
231 const Vector& source) const
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
232 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
233 assert(source.size() == 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
234 const Vector q = source - c_;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
235
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
236 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
237 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
238 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
239
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
240 LinearAlgebra::AssignVector(target, x, y, z);
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
241 }
146
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
242
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
243
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
244 bool OrientedBoundingBox::ComputeExtent(Extent2D& extent,
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
245 const CoordinateSystem3D& plane) const
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
246 {
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
247 extent.Reset();
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 std::vector<Vector> points;
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
250 if (HasIntersection(points, plane))
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
251 {
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
252 for (size_t i = 0; i < points.size(); i++)
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 double x, y;
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
255 plane.ProjectPoint(x, y, points[i]);
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
256 extent.AddPoint(x, y);
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
257 }
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
258
fb7d602e7025 OrientedBoundingBox::ComputeExtent
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 140
diff changeset
259 return true;
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 else
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 return false;
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 }
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
266 }