core/vgl/vgl_homg_point_3d.txx Source File

Go to the documentation of this file.
00001 // This is core/vgl/vgl_homg_point_3d.txx
00002 #ifndef vgl_homg_point_3d_txx_
00003 #define vgl_homg_point_3d_txx_
00004 
00005 #include "vgl_homg_point_3d.h"
00006 #include <vgl/vgl_homg_plane_3d.h>
00007 #include <vcl_iostream.h>
00008 
00009 // Note that the given planes must be distinct and not have a line in common!
00010 template <class Type>
00011 vgl_homg_point_3d<Type>::vgl_homg_point_3d (vgl_homg_plane_3d<Type> const& l1,
00012                                             vgl_homg_plane_3d<Type> const& l2,
00013                                             vgl_homg_plane_3d<Type> const& l3)
00014 {
00015   set(-l1.ny()*l2.nz()*l3.d()-l2.ny()*l3.nz()*l1.d()-l3.ny()*l1.nz()*l2.d()
00016       +l1.ny()*l3.nz()*l2.d()+l2.ny()*l1.nz()*l3.d()+l3.ny()*l2.nz()*l1.d(),
00017        l1.nz()*l2.d()*l3.nx()+l2.nz()*l3.d()*l1.nx()+l3.nz()*l1.d()*l2.nx()
00018       -l1.nz()*l3.d()*l2.nx()-l2.nz()*l1.d()*l3.nx()-l3.nz()*l2.d()*l1.nx(),
00019       -l1.d()*l2.nx()*l3.ny()-l2.d()*l3.nx()*l1.ny()-l3.d()*l1.nx()*l2.ny()
00020       +l1.d()*l3.nx()*l2.ny()+l2.d()*l1.nx()*l3.ny()+l3.d()*l2.nx()*l1.ny(),
00021        l1.nx()*l2.ny()*l3.nz()+l2.nx()*l3.ny()*l1.nz()+l3.nx()*l1.ny()*l2.nz()
00022       -l1.nx()*l3.ny()*l2.nz()-l2.nx()*l1.ny()*l3.nz()-l3.nx()*l2.ny()*l1.nz());
00023 }
00024 
00025 template <class Type>
00026 bool vgl_homg_point_3d<Type>::operator==(vgl_homg_point_3d<Type> const& other) const
00027 {
00028   return (this==&other) ||
00029          (x()*other.y() == y()*other.x() &&
00030           x()*other.z() == z()*other.x() &&
00031           x()*other.w() == w()*other.x() &&
00032           y()*other.z() == z()*other.y() &&
00033           y()*other.w() == w()*other.y() &&
00034           z()*other.w() == w()*other.z());
00035 }
00036 
00037 template <class Type>
00038 bool collinear(vgl_homg_point_3d<Type> const& p1,
00039                vgl_homg_point_3d<Type> const& p2,
00040                vgl_homg_point_3d<Type> const& p3)
00041 {
00042   if (!p1.ideal() && !p2.ideal() && !p3.ideal())
00043     return parallel(p1-p2, p1-p3);
00044   if (!p1.ideal() && !p2.ideal() && p3.ideal())
00045     return parallel(p1-p2, vgl_vector_3d<Type>(p3.x(),p3.y(),p3.z()));
00046   if (!p1.ideal() && p2.ideal() && !p3.ideal())
00047     return parallel(p1-p3, vgl_vector_3d<Type>(p2.x(),p2.y(),p2.z()));
00048   if (p1.ideal() && !p2.ideal() && !p3.ideal())
00049     return parallel(p2-p3, vgl_vector_3d<Type>(p1.x(),p1.y(),p1.z()));
00050   if (p1.ideal() && p2.ideal() && !p3.ideal())
00051     return false;
00052   if (p1.ideal() && !p2.ideal() && p3.ideal())
00053     return false;
00054   if (!p1.ideal() && p2.ideal() && p3.ideal())
00055     return false;
00056   // all three are ideal:
00057   return (p1.x()*p2.y()-p1.y()*p2.x())*p3.z()
00058         +(p3.x()*p1.y()-p3.y()*p1.x())*p2.z()
00059         +(p2.x()*p3.y()-p2.y()*p3.x())*p1.z()==0;
00060 }
00061 
00062 template <class T>
00063 double cross_ratio(vgl_homg_point_3d<T>const& p1, vgl_homg_point_3d<T>const& p2,
00064                    vgl_homg_point_3d<T>const& p3, vgl_homg_point_3d<T>const& p4)
00065 {
00066   // least squares solution: (Num_x-CR*Den_x)^2 + (Num_y-CR*Den_y)^2 + (Num_z-CR*Den_z)^2 minimal.
00067   double Num_x = (p1.x()*p3.w()-p3.x()*p1.w())*(p2.x()*p4.w()-p4.x()*p2.w());
00068   double Num_y = (p1.y()*p3.w()-p3.y()*p1.w())*(p2.y()*p4.w()-p4.y()*p2.w());
00069   double Num_z = (p1.z()*p3.w()-p3.z()*p1.w())*(p2.z()*p4.w()-p4.z()*p2.w());
00070   double Den_x = (p1.x()*p4.w()-p4.x()*p1.w())*(p2.x()*p3.w()-p3.x()*p2.w());
00071   double Den_y = (p1.y()*p4.w()-p4.y()*p1.w())*(p2.y()*p3.w()-p3.y()*p2.w());
00072   double Den_z = (p1.z()*p4.w()-p4.z()*p1.w())*(p2.z()*p3.w()-p3.z()*p2.w());
00073   if (Den_x == Den_y && Den_y == Den_z) return (Num_x+Num_y+Num_z)/3/Den_x;
00074   else return (Den_x*Num_x+Den_y*Num_y+Den_z*Num_z)/(Den_x*Den_x+Den_y*Den_y+Den_z*Den_z);
00075 }
00076 
00077 template <class Type>
00078 vcl_ostream& operator<<(vcl_ostream& s, vgl_homg_point_3d<Type> const& p)
00079 {
00080   return s << " <vgl_homg_point_3d ("
00081            << p.x() << ',' << p.y() << ','
00082            << p.z() << ',' << p.w() << ") >";
00083 }
00084 
00085 template <class Type>
00086 vcl_istream& operator>>(vcl_istream& s, vgl_homg_point_3d<Type>& p)
00087 {
00088   Type x, y, z, w;
00089   s >> x >> y >> z >> w;
00090   p.set(x,y,z,w);
00091   return s;
00092 }
00093 
00094 #undef VGL_HOMG_POINT_3D_INSTANTIATE
00095 #define VGL_HOMG_POINT_3D_INSTANTIATE(T) \
00096 template class vgl_homg_point_3d<T >; \
00097 template bool collinear(vgl_homg_point_3d<T >const&,vgl_homg_point_3d<T >const&,vgl_homg_point_3d<T >const&); \
00098 template double cross_ratio(vgl_homg_point_3d<T >const&, vgl_homg_point_3d<T >const&, \
00099                             vgl_homg_point_3d<T >const&, vgl_homg_point_3d<T >const&); \
00100 template vcl_ostream& operator<<(vcl_ostream&, vgl_homg_point_3d<T >const&); \
00101 template vcl_istream& operator>>(vcl_istream&, vgl_homg_point_3d<T >&)
00102 
00103 #endif // vgl_homg_point_3d_txx_