DualCoding: Point.h Source File

00001 //-*-c++-*-
00002 #ifndef _POINT_H_
00003 #define _POINT_H_
00004 
00005 #include <iostream>
00006 #include <vector>
00007 
00008 #include "Shared/newmat/newmat.h"
00009 #include "Shared/newmat/newmatio.h"
00010 
00011 #include "Measures.h"   // coordinate_t, AngPi
00012 #include "ShapeTypes.h" // ReferenceFrameType_t
00013 
00014 namespace DualCoding {
00015 
00016 class LineData;
00017 template<typename T> class Shape;
00018 
00019 /*! We define Point as a separate lightweight class because it is used
00020  * as a component of all the xxxData classes, and we don't want to nest
00021  * these structures.
00022  */
00023 
00024 class Point {
00025 
00026 public:
00027   NEWMAT::ColumnVector coords;
00028   ReferenceFrameType_t refFrameType;
00029 
00030   //! Constructors
00031   //@{
00032   Point(void);
00033   Point(coordinate_t const &xp, coordinate_t const &yp, coordinate_t zp=0, ReferenceFrameType_t ref=unspecified);
00034   Point(const NEWMAT::ColumnVector& c, ReferenceFrameType_t ref=unspecified);
00035   //@}
00036 
00037   //! Copy constructor
00038   Point(const Point& otherPt) : coords(otherPt.coords), refFrameType(otherPt.refFrameType) {}
00039 
00040   //! Destructor
00041   virtual ~Point() { }
00042 
00043   NEWMAT::ColumnVector& getCoords() const { return const_cast<Point*>(this)->coords; }
00044   coordinate_t coordX() const { return coords(1); }
00045   coordinate_t coordY() const { return coords(2); }
00046   coordinate_t coordZ() const { return coords(3); }
00047 
00048   //! Set position.
00049   //@{
00050   void setCoords(const Point& otherPt);
00051   void setCoords(coordinate_t _x, coordinate_t _y, coordinate_t z=0);
00052   //@}
00053 
00054   //! Set reference frame type
00055   void setRefFrameType(const ReferenceFrameType_t ref) { refFrameType = ref; }
00056 
00057   //! Euclidean distance from another point to this one
00058   float distanceFrom(const Point& other) const;
00059   float xyDistanceFrom(const Point& other) const;
00060 
00061 
00062   //! These functions need a ShapeSpace argument because left/right depends on reference frame type.
00063   //@{
00064   bool isLeftOf(const Point& other, float distance=0) const;
00065   bool isRightOf(const Point& other, float distance=0) const;
00066   bool isAbove(const Point& other, float distance=0) const;
00067   bool isBelow(const Point& other, float distance=0) const;
00068 //@}
00069 
00070   //! These functions return true based on relative positions, assuming points line in a 2D plane (z coordinate is ignored).
00071   //@{
00072   bool isBetween(const Point& other1, const Point& other2) const;
00073   bool isBetween(const Shape<LineData>& line1, const Shape<LineData>& line2) const;
00074   bool isBetween(const LineData& line1, const LineData& line2) const;
00075   bool isInside(const vector<LineData>& bound) const;
00076 //@}
00077 
00078   void applyTransform(const NEWMAT::Matrix& T);
00079   
00080   float getHeightAbovePoint(const Point& groundPoint, const NEWMAT::ColumnVector& groundplane);
00081   
00082 
00083   //! projects this to ground plane according to camToBase matrix
00084   void projectToGround(const NEWMAT::Matrix& camToBase,
00085            const NEWMAT::ColumnVector& groundPlane);
00086 
00087   void projectToGround(int xres, int yres,
00088            const NEWMAT::ColumnVector& ground_plane);
00089 
00090   Point operator+(const Point& b) const;
00091   Point& operator+=(const Point& b);
00092 
00093   Point operator-(const Point& b) const;
00094   Point& operator-=(const Point& b);
00095 
00096   Point operator*(float b) const;
00097   Point& operator*=(float b);
00098 
00099   Point operator/(float b) const;
00100   Point& operator/=(float b);
00101 
00102   bool operator==(const Point& b) const;
00103   bool operator!=(const Point& b) const { return !operator==(b); }
00104 
00105   Point& operator=(const Point& b) {
00106     if (&b==this) return *this;
00107     setCoords(b);
00108     refFrameType = b.refFrameType;
00109     return *this;
00110   }
00111 
00112   void printData();
00113 
00114   friend class EndPoint;
00115 
00116 };
00117 
00118 inline Point& leftMost(Point &pt1, Point &pt2) { return pt1.isLeftOf(pt2) ? pt1 : pt2; }
00119 inline Point& rightMost(Point &pt1, Point &pt2) { return pt1.isLeftOf(pt2) ? pt2 : pt1; }
00120 inline Point& topMost(Point &pt1, Point &pt2) { return pt1.isAbove(pt2) ? pt1 : pt2; }
00121 inline Point& bottomMost(Point &pt1, Point &pt2) { return pt1.isAbove(pt2) ? pt2 : pt1; }
00122 
00123 std::ostream& operator<<(std::ostream &os, const Point &p);
00124 
00125 } // namespace
00126 
00127 #endif