00001
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 "Shared/Measures.h"
00012 #include "ShapeTypes.h"
00013
00014 namespace DualCoding {
00015
00016 class LineData;
00017 template<typename T> class Shape;
00018
00019
00020
00021
00022
00023
00024 class Point {
00025
00026 public:
00027 NEWMAT::ColumnVector coords;
00028 ReferenceFrameType_t refFrameType;
00029
00030
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
00038 Point(const Point& otherPt) : coords(otherPt.coords), refFrameType(otherPt.refFrameType) {}
00039
00040
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 ReferenceFrameType_t getRefFrameType() const { return refFrameType; }
00048
00049
00050
00051 void setCoords(const Point& otherPt);
00052 void setCoords(coordinate_t _x, coordinate_t _y, coordinate_t z=0);
00053
00054
00055
00056 void setRefFrameType(const ReferenceFrameType_t ref) { refFrameType = ref; }
00057
00058
00059 float distanceFrom(const Point& other) const;
00060 float xyDistanceFrom(const Point& other) const;
00061 float xyNorm() const;
00062
00063
00064
00065 bool isLeftOf(const Point& other, float distance=0) const;
00066 bool isRightOf(const Point& other, float distance=0) const;
00067 bool isAbove(const Point& other, float distance=0) const;
00068 bool isBelow(const Point& other, float distance=0) const;
00069
00070
00071
00072
00073 bool isBetween(const Point& other1, const Point& other2) const;
00074 bool isBetween(const Shape<LineData>& line1, const Shape<LineData>& line2) const;
00075 bool isBetween(const LineData& line1, const LineData& line2) const;
00076 bool isInside(const std::vector<LineData>& bound) const;
00077
00078
00079 float getHeightAbovePoint(const Point& groundPoint, const NEWMAT::ColumnVector& groundplane);
00080
00081
00082 void applyTransform(const NEWMAT::Matrix& T, const ReferenceFrameType_t newref=unspecified);
00083
00084
00085 void projectToGround(const NEWMAT::Matrix& camToBase,
00086 const NEWMAT::ColumnVector& groundPlane);
00087
00088 void projectToGround(int xres, int yres,
00089 const NEWMAT::ColumnVector& ground_plane);
00090
00091 Point operator+(const Point& b) const;
00092 Point& operator+=(const Point& b);
00093
00094 Point operator-(const Point& b) const;
00095 Point& operator-=(const Point& b);
00096
00097 Point operator*(float b) const;
00098 Point& operator*=(float b);
00099
00100 Point operator/(float b) const;
00101 Point& operator/=(float b);
00102
00103 bool operator==(const Point& b) const;
00104 bool operator!=(const Point& b) const { return !operator==(b); }
00105
00106 Point& operator=(const Point& b) {
00107 if (&b==this) return *this;
00108 setCoords(b);
00109 refFrameType = b.refFrameType;
00110 return *this;
00111 }
00112
00113 void printData();
00114
00115 friend class EndPoint;
00116
00117 private:
00118 void makeRefFrameCompatible(const Point &other);
00119
00120 };
00121
00122 inline Point& leftMost(Point &pt1, Point &pt2) { return pt1.isLeftOf(pt2) ? pt1 : pt2; }
00123 inline Point& rightMost(Point &pt1, Point &pt2) { return pt1.isLeftOf(pt2) ? pt2 : pt1; }
00124 inline Point& topMost(Point &pt1, Point &pt2) { return pt1.isAbove(pt2) ? pt1 : pt2; }
00125 inline Point& bottomMost(Point &pt1, Point &pt2) { return pt1.isAbove(pt2) ? pt2 : pt1; }
00126
00127 std::ostream& operator<<(std::ostream &os, const Point &p);
00128
00129 }
00130
00131 #endif