00001
00002 #ifndef vgl_orient_box_3d_txx_
00003 #define vgl_orient_box_3d_txx_
00004
00005
00006
00007 #include "vgl_orient_box_3d.h"
00008 #include <vgl/vgl_vector_3d.h>
00009 #include <vnl/vnl_det.h>
00010 #include <vcl_iostream.h>
00011
00012
00013
00014 template <class Type>
00015 vgl_orient_box_3d<Type>::vgl_orient_box_3d(vgl_point_3d<Type> const& p0,
00016 vgl_point_3d<Type> const& px,
00017 vgl_point_3d<Type> const& py,
00018 vgl_point_3d<Type> const& pz)
00019 {
00020
00021 vgl_vector_3d<Type> vx = px - p0, vy = py - p0, vz = pz - p0;
00022 double lx = vx.length(), ly = vy.length(), lz = vz.length();
00023
00024 vnl_matrix_fixed<double,3,3> rotation;
00025
00026 rotation(0,0) = vx.x()/lx; rotation(0,1) = vx.y()/lx; rotation(0,2) = vx.z()/lx;
00027 rotation(1,0) = vy.x()/ly; rotation(1,1) = vy.y()/ly; rotation(1,2) = vy.z()/ly;
00028 rotation(2,0) = vz.x()/lz; rotation(2,1) = vz.y()/lz; rotation(2,2) = vz.z()/lz;
00029
00030 double det = vnl_det(rotation);
00031 if (det < 0) {
00032 double t = rotation(0,0); rotation(0,0) = rotation(1,0); rotation(1,0) = t;
00033 t = rotation(0,1); rotation(0,1) = rotation(1,1); rotation(1,1) = t;
00034 t = rotation(0,2); rotation(0,2) = rotation(1,2); rotation(1,2) = t;
00035 }
00036 orient_ = vnl_quaternion<double>(rotation);
00037
00038
00039 vnl_vector_fixed<double,3> centroid(-0.5*p0.x()+0.5*px.x()+0.5*py.x()+0.5*pz.x(),
00040 -0.5*p0.y()+0.5*px.y()+0.5*py.y()+0.5*pz.y(),
00041 -0.5*p0.z()+0.5*px.z()+0.5*py.z()+0.5*pz.z());
00042 vnl_vector_fixed<double,3> p0_to_centroid = vnl_vector_fixed<double,3>(p0.x(),p0.y(),p0.z()) - centroid;
00043 vnl_vector_fixed<double,3> corner_0 = centroid + rotation * p0_to_centroid;
00044 vgl_point_3d<Type> corner0((Type)(corner_0[0]), (Type)(corner_0[1]), (Type)(corner_0[2]));
00045 vnl_vector_fixed<double,3> corner_1 = centroid - rotation * p0_to_centroid;
00046 vgl_point_3d<Type> corner1((Type)(corner_1[0]), (Type)(corner_1[1]), (Type)(corner_1[2]));
00047 box_ = vgl_box_3d<Type>(corner0, corner1);
00048 }
00049
00050
00051 template <class Type>
00052 vcl_vector<vgl_point_3d<Type> > vgl_orient_box_3d<Type>::corners()
00053 {
00054 vcl_vector<vgl_point_3d<Type> > corner_points(8);
00055
00056
00057 corner_points[0] = box_.min_point();
00058 corner_points[7] = box_.max_point();
00059
00060 corner_points[1] = vgl_point_3d<Type> (corner_points[0].x()+width(),
00061 corner_points[0].y(),
00062 corner_points[0].z());
00063 corner_points[2] = vgl_point_3d<Type> (corner_points[0].x(),
00064 corner_points[0].y(),
00065 corner_points[0].z()+depth());
00066 corner_points[3] = vgl_point_3d<Type> (corner_points[1].x(),
00067 corner_points[1].y(),
00068 corner_points[1].z()+depth());
00069 corner_points[4] = vgl_point_3d<Type> (corner_points[0].x(),
00070 corner_points[0].y()+height(),
00071 corner_points[0].z());
00072 corner_points[5] = vgl_point_3d<Type> (corner_points[1].x(),
00073 corner_points[1].y()+height(),
00074 corner_points[1].z());
00075 corner_points[6] = vgl_point_3d<Type> (corner_points[2].x(),
00076 corner_points[2].y()+height(),
00077 corner_points[2].z());
00078
00079 for (unsigned int i=0; i < corner_points.size(); i++) {
00080 vnl_vector_fixed<double,3> p;
00081 p[0] = corner_points[i].x() - box_.centroid_x();
00082 p[1] = corner_points[i].y() - box_.centroid_y();
00083 p[2] = corner_points[i].z() - box_.centroid_z();
00084 p = orient_.rotate(p);
00085 corner_points[i] = vgl_point_3d<Type> (Type(p[0]) + box_.centroid_x(),
00086 Type(p[1]) + box_.centroid_y(),
00087 Type(p[2]) + box_.centroid_z());
00088 }
00089 return corner_points;
00090 }
00091
00092
00093 template <class Type>
00094 bool vgl_orient_box_3d<Type>::contains(Type const& x,
00095 Type const& y,
00096 Type const& z) const
00097 {
00098
00099 vnl_quaternion<double> reverse_rot = orient_.inverse();
00100
00101 vnl_vector_fixed<double,3> p;
00102 p[0] = x - box_.centroid_x();
00103 p[1] = y - box_.centroid_y();
00104 p[2] = z - box_.centroid_z();
00105 p = reverse_rot.rotate(p);
00106 Type xt = static_cast<Type>(p[0] + box_.centroid_x());
00107 Type yt = static_cast<Type>(p[1] + box_.centroid_y());
00108 Type zt = static_cast<Type>(p[2] + box_.centroid_z());
00109 return box_.contains(xt, yt, zt);
00110 }
00111
00112 template <class Type>
00113 vcl_ostream& vgl_orient_box_3d<Type>::print(vcl_ostream& s) const
00114 {
00115 return s << "<vgl_orient_box_3d " << box_ << " dir=" << orient_ << '>' << vcl_endl;
00116 }
00117
00118 template <class Type>
00119 vcl_istream& vgl_orient_box_3d<Type>::read(vcl_istream& s)
00120 {
00121 return s >> box_ >> orient_ ;
00122 }
00123
00124 //: Write box to stream
00125 template <class Type>
00126 vcl_ostream& operator<<(vcl_ostream& s, vgl_orient_box_3d<Type> const& p)
00127 {
00128 return p.print(s);
00129 }
00130
00131 //: Read box from stream
00132 template <class Type>
00133 vcl_istream& operator>>(vcl_istream& is, vgl_orient_box_3d<Type>& p)
00134 {
00135 return p.read(is);
00136 }
00137
00138 #undef VGL_ORIENT_BOX_3D_INSTANTIATE
00139 #define VGL_ORIENT_BOX_3D_INSTANTIATE(T) \
00140 template class vgl_orient_box_3d<T >;\
00141 template vcl_ostream& operator<<(vcl_ostream&, vgl_orient_box_3d<T > const& p);\
00142 template vcl_istream& operator>>(vcl_istream&, vgl_orient_box_3d<T >& p)
00143
00144 #endif // vgl_orient_box_3d_txx_