00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 #ifndef AMOD_MATH_LIB_INCLINE3D_H
00024 #define AMOD_MATH_LIB_INCLINE3D_H
00025 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00037 
00038 #include "point3d.H"
00039 
00040 template <class L, class P, class V>
00041 class _line
00042 {
00043 protected : 
00044     P  _point;
00045     V  _vector;
00046 
00047 public:
00048     _line()                                                       {}
00049     _line(const P&  p, const V& v)  : _point ( p), _vector(v)     {}
00050     _line(const P& p1, const P& p2) : _point (p1), _vector(p2-p1) {}
00051 
00052     P        project(const P&p)  const { V vn(_vector.normalize()); 
00053                                          return _point + ((p-_point)*vn) *vn; }
00054     Greal   dist   (const P&p)  const { if (!isValid()){cerr<<"Invalid line\n";
00055                                                       return _point.dist(p);}
00056                                          return project(p).dist(p); }
00057 
00058     const P &point ()               const  { return _point; }
00059     P       &point ()                      { return _point; }
00060     const V &vector()               const  { return _vector; }
00061     V       &vector()                      { return _vector; }
00062 
00063     bool    isValid()               const  { return !_vector.isNull(); }
00064 
00065     P       intersect(const L &l)   const  { 
00066             const P &A = point();            
00067             const P &B = l.point();          
00068             const V  Y = vector().normalize();
00069             const V  W = l.vector().normalize();
00070             const V BA = B-A;
00071             const Greal vv = Y*Y;
00072             const Greal ww = W*W;
00073             const Greal vw = Y*W;
00074             const Greal vba= Y*BA;
00075             const Greal wba= W*BA;
00076             const Greal det = vw*vw-vv*ww;
00077 
00078         if (det == 0)
00079            return project(A + (B-A) * .5);
00080 
00081         const Greal as = (vw*wba-ww*vba)/det;
00082         const P      AP = A+as*Y;
00083 
00084         return AP;
00085     }
00086     int operator==(const _line<L,P,V> &l) const {
00087        return l._point == _point && l._vector == _vector;
00088     }
00089 };  
00090 
00091 
00092 
00093 template <class L, class P, class V>
00094 ostream &operator<<(ostream &os, const _line<L,P,V>  &l) 
00095                    { return os << "Line("<<l.point()<<","<<l.vector()<<")"; }
00096 
00097 
00098 #endif
00099