1 #ifndef vgl_frustum_3d_hxx_
2 #define vgl_frustum_3d_hxx_
3
4 #include <iostream>
5 #include <cmath>
6 #include "vgl_frustum_3d.h"
7 #include "vgl_ray_3d.h"
8 #include "vgl_plane_3d.h"
9 #include "vgl_tolerance.h"
10 #include "vgl_intersection.h"
11 #ifdef _MSC_VER
12 # include <vcl_msvc_warnings.h>
13 #endif
14 //:
15 template <class Type>
16 vgl_frustum_3d<Type>::
vgl_frustum_3d(std::vector<vgl_ray_3d<Type>> const & corner_rays,vgl_vector_3d<Type> const & norm,Type d0,Type d1)17 vgl_frustum_3d(std::vector<vgl_ray_3d<Type> > const& corner_rays,
18 vgl_vector_3d<Type> const& norm, Type d0, Type d1){
19
20 //Construct cone bounding planes. Their surface normals point outward from
21 //the interior of the frustum. It is assumed the rays are in order
22 //around the boundaries of the parallel faces, so that adjacent rays
23 //define the cone surface planes with outward pointing normals
24 int nc = static_cast<int>(corner_rays.size());
25 assert(nc>=3);//need to enclose a volume
26 n_top_bot_face_verts_= nc;
27 norm_ = norm;
28 // contruct the near plane
29 vgl_ray_3d<Type> r0 = corner_rays[0];
30 apex_ = r0.origin();
31 // find a point on the near plane
32 vgl_vector_3d<Type> dir = r0.direction();
33 // ray shouldn't be perpendicular to near plane normal
34 double er = std::fabs(cos_angle(norm, dir));
35 assert(er > vgl_tolerance<double>::position);
36 double dp = dot_product(norm, dir);
37 // the normal to the near parallel face must point to the apex
38 // since the apex is, by defintion, outside the frustum volume
39 vgl_vector_3d<Type> snorm = norm;
40 if(dp>0) snorm = -norm;
41 double dp_mag = std::fabs(dp);
42 double tau_near = d0/dp_mag;
43 vgl_point_3d<Type> p_near = apex_ + (tau_near*dir);
44 vgl_plane_3d<Type> pln_near(snorm, p_near);
45 near_plane_ = static_cast<int>(surface_planes_.size());
46 surface_planes_.push_back(pln_near);
47
48 //define the verts on the near plane
49 verts_.push_back(p_near);
50 for(int i = 1; i<nc; ++i){
51 // find the intersection point of ri, i>0, with the top plane
52 vgl_point_3d<Type> pint;
53 bool good = vgl_intersection<Type>(corner_rays[i], pln_near, pint);
54 assert(good);
55 verts_.push_back(pint);
56 }
57 // check order
58 vgl_vector_3d<Type> v01 = verts_[1]-verts_[0];
59 vgl_vector_3d<Type> v12 = verts_[2]-verts_[1];
60 vgl_vector_3d<Type> out = cross_product(v01, v12);
61 bool rays_ccw_top = dot_product(norm, out) > 0;
62 if(rays_ccw_top)
63 for(int i = 0; i<nc; ++i)
64 faces_[near_plane_].push_back(i);
65 else
66 for(int i = nc-1; i>=0; --i)
67 faces_[near_plane_].push_back(i);
68 // for now, the test for inside assumes a convex frustum FIX_ME
69 assert(this->is_convex());
70 // define the far plane
71 double tau_far = d1/dp_mag;
72 vgl_point_3d<Type> p_far = apex_ + (tau_far*dir);
73 int far_indx = static_cast<int>(verts_.size());
74 verts_.push_back(p_far);
75 // the normal to the far parallel face must point away from the apex
76 vgl_plane_3d<Type> pln_far(-snorm, p_far);
77 far_plane_ = static_cast<int>(surface_planes_.size());
78 surface_planes_.push_back(pln_far);
79 // compute the verts on the far plane
80 for(int i = 1; i<nc; ++i){
81 // find the intersection point of ri, i>0, with the top plane
82 vgl_point_3d<Type> pi;
83 bool good = vgl_intersection<Type>(corner_rays[i], pln_far, pi);
84 assert(good);
85 verts_.push_back(pi);
86 }
87 // form the far face. order is reversed on far face so normal is pointing out
88 if(rays_ccw_top)
89 for(int i = nc-1; i>=0; --i)
90 faces_[far_plane_].push_back(i+far_indx);
91 else
92 for(int i = 0; i<nc; ++i)
93 faces_[near_plane_].push_back(i+far_indx);
94
95 // find the side cone planes and associated face verts
96 // each side face has four vertices, two on the near face and two
97 // on the far face
98 std::vector<int>& top_verts = faces_[near_plane_];
99 for(int i = 0; i<nc; ++i){
100 int j = top_verts[i];
101 int j_next = top_verts[(i+1)%nc];
102 const vgl_ray_3d<Type>& ra = corner_rays[j];
103 const vgl_ray_3d<Type>& rb = corner_rays[j_next];
104 vgl_plane_3d<Type> pln(ra, rb);
105 int pln_index = static_cast<int>(surface_planes_.size());
106 surface_planes_.push_back(pln);
107 faces_[pln_index].push_back(j); faces_[pln_index].push_back(j_next);
108 faces_[pln_index].push_back(j_next+far_indx);
109 faces_[pln_index].push_back(j+far_indx);
110 }
111 }
112
113 template <class Type>
operator ==(vgl_frustum_3d<Type> const & other) const114 bool vgl_frustum_3d<Type>::operator==(vgl_frustum_3d<Type> const& other) const{
115 // check addresses
116 if(this == &other)
117 return true;
118 // check apex
119 if(!(apex_ == other.apex()))
120 return false;
121 int n = static_cast<int>(verts_.size());
122 const std::vector<vgl_point_3d<Type> >& o_verts = other.verts();
123 // could be round off error
124 for(int i = 0; i<n; ++i){
125 double dif = (verts_[i] - o_verts[i]).length();
126 if( dif > vgl_tolerance<double>::position)
127 return false;
128 }
129 return true;
130 }
131 template <class Type>
bounding_box() const132 vgl_box_3d<Type> vgl_frustum_3d<Type>::bounding_box() const{
133 vgl_box_3d<Type> box;
134 int n = static_cast<int>(verts_.size());
135 for(int i = 0; i<n; ++i)
136 box.add(verts_[i]);
137 return box;
138 }
139 template <class Type>
centroid() const140 vgl_point_3d<Type> vgl_frustum_3d<Type>::centroid() const{
141 int n = static_cast<int>(verts_.size());
142 double x_sum = 0.0, y_sum = 0.0, z_sum = 0.0;
143 for(int i = 0; i<n; ++i){
144 x_sum += static_cast<double>(verts_[i].x());
145 y_sum += static_cast<double>(verts_[i].y());
146 z_sum += static_cast<double>(verts_[i].z());
147 }
148 x_sum /= n; y_sum /= n; z_sum /= n;
149 Type x = static_cast<Type>(x_sum);
150 Type y = static_cast<Type>(y_sum);
151 Type z = static_cast<Type>(z_sum);
152 return vgl_point_3d<Type>(x, y, z);
153 }
154 // traverse the top face and compute the cross product of sequential
155 // face edges
156 template <class Type>
is_convex() const157 bool vgl_frustum_3d<Type>::is_convex() const{
158 int n = n_top_bot_face_verts_;
159 if(n<3) return false;
160 if(n==3) return true;
161 // n > 3
162 // get the vertex map for the top face
163 std::map<int, std::vector<int> >::const_iterator vit;
164 vit = faces_.find(near_plane_);//use find since [] is non-const
165 if(vit==faces_.end()) return false;
166 const std::vector<int>& vindx = (*vit).second;
167 vgl_vector_3d<Type> v = verts_[vindx[1]]-verts_[vindx[0]];
168 vgl_vector_3d<Type> v_pre = verts_[vindx[2]]-verts_[vindx[1]];
169 vgl_vector_3d<Type> cr = cross_product(v, v_pre);
170 Type dp = dot_product(cr, norm_);
171 bool pos = dp > vgl_tolerance<Type>::position;
172 for(int i = 2; i<n; ++i){
173 int j = (vindx[i]+1)%n;
174 vgl_vector_3d<Type> v_nxt = verts_[j]-verts_[vindx[i]];
175 cr = cross_product(v_pre, v_nxt);
176 dp = dot_product(cr, norm_);
177 bool pos_i = dp > vgl_tolerance<Type>::position;
178 if(pos_i != pos) return false;
179 v_pre = v_nxt;
180 }
181 return true;
182 }
183
184 template <class Type>
185 bool vgl_frustum_3d<Type>::
contains(Type const & x,Type const & y,Type const & z) const186 contains(Type const& x, Type const& y, Type const& z) const{
187
188 // the point must be on the inside of all the faces,
189 // assuming that the fustrum is a convex solid.
190 int n = static_cast<int>(surface_planes_.size());
191 bool inside = true;
192 for(int i = 0; i<n&&inside; ++i){
193 Type a = surface_planes_[i].a(), b = surface_planes_[i].b();
194 Type c = surface_planes_[i].c(), d = surface_planes_[i].d();
195 Type sign = a*x + b*y + c*z +d;
196 inside = sign < vgl_tolerance<Type>::position;
197 }
198 return inside;
199 }
200
201 template <class Type>
contains(vgl_point_3d<Type> const & p) const202 bool vgl_frustum_3d<Type>::contains(vgl_point_3d<Type> const& p) const{
203 return contains(p.x(), p.y(), p.z());
204 }
205
206 template <class Type>
operator <<(std::ostream & s,vgl_frustum_3d<Type> const & f)207 std::ostream& operator<<(std::ostream& s, vgl_frustum_3d<Type> const& f){
208 s << "<vgl_frustum_3d [\n";
209 const std::vector<vgl_point_3d<Type> >& verts = f.verts();
210 int n = static_cast<int>(verts.size());
211 for(int i = 0; i<n; ++i)
212 s << verts[i] << '\n';
213 s << "] >\n";
214 return s;
215 }
216 #undef VGL_FRUSTUM_3D_INSTANTIATE
217 #define VGL_FRUSTUM_3D_INSTANTIATE(Type) \
218 template class vgl_frustum_3d<Type >;\
219 template std::ostream& operator<<(std::ostream&, vgl_frustum_3d<Type > const& f)
220
221 #endif //vgl_frustum_3d_hxx_
222