Measures.h

Go to the documentation of this file.
00001 #ifndef INCLUDED_Measures_h_
00002 #define INCLUDED_Measures_h_
00003 
00004 #include "Shared/fmatCore.h"
00005 
00006 #include <cmath>
00007 
00008 //! Representation of a plane as ax + by + cz = d; used for representing the ground plane
00009 class PlaneEquation {
00010 private:
00011   fmat::Column<3> direction;  //!< Unit vector normal to the plane
00012   float displacement; //!< distance along normal from origin
00013 public:
00014   PlaneEquation(const fmat::Column<3> &dir, float disp); //!< constructor, will normalize @a dir and apply to @a disp as well to maintain relative relationship
00015   PlaneEquation(float a, float b, float c, float d); //!< constructor, pass plane equation representing ax + by + cz = d
00016   PlaneEquation() : direction(fmat::pack(0.f, 0.f, 1.f)), displacement(0) {} //!< constructor, assumes upward z normal
00017   const fmat::Column<3>& getDirection() const { return direction; } //!< returns #direction
00018   void setDirection(const fmat::Column<3>& dir) { direction = dir / dir.norm(); } //!< normalizes and assigns #direction (normalization here does not affect #displacement, unlike constructor)
00019   
00020   float getDisplacement() const { return displacement; } //!< returns #displacement
00021   void setDisplacement(float disp) { displacement = disp; } //!< assigns #displacement
00022 
00023   float getZsign() const { return direction[2] < 0 ? -1.f : 1.f; } //!< returns sign on #displacement
00024   float getDsign() const { return displacement < 0 ? -1.f : 1.f; } //!< returns sign on #displacement
00025   
00026   //! assign to a four-element vector via operator[], with displacement in the last element ([3])
00027   template<class T> void exportTo(T& x) const { x[0]=direction[0]; x[1]=direction[1]; x[2]=direction[2]; x[3]=displacement; }
00028   //! pull values from a four-element vector via operator[], with displacement in the last element ([3])
00029   template<class T> void importFrom(const T& x) { direction[0]=x[0]; direction[1]=x[1]; direction[2]=x[2]; displacement=x[3]; }
00030 };
00031 
00032 std::ostream& operator<<(std::ostream &os, const PlaneEquation& p);
00033 
00034 typedef float coordinate_t; //!< type used for positional coordinates
00035 typedef float orientation_t; //!< type used for orientation values (0 to Pi)
00036 typedef float direction_t; //!< type used for direction values (0 to 2*Pi)
00037 
00038 const direction_t Pi=static_cast<direction_t>(M_PI); //!< shorthand for ::M_PI from math.h
00039 const direction_t TwoPi=static_cast<direction_t>(2*M_PI); //!< shorthand for 2*M_PI 
00040 
00041 typedef coordinate_t Slope; //!< type used for ratio of coordinate offsets
00042 const Slope BIG_SLOPE=static_cast<Slope>(5000); //!< slopes larger than this are considered vertical, or in other words, infinite slopes are rounded to this
00043 
00044 //! Circular arithmetic on angles between 0 and pi (180 degrees)
00045 class AngPi {
00046   friend AngPi angdist(AngPi const &arg1, AngPi const &arg2);
00047 public:
00048   AngPi(void) : value(0) {}; //!< constructor, #value defaults to 0
00049   AngPi(orientation_t const &v) : value(v) { normalize(); } //!< conversion operator allows implicit construction from primitive
00050   
00051   AngPi operator+(AngPi const &arg) const { return AngPi(value+arg.value); };
00052   AngPi operator-(AngPi const &arg) const { return AngPi(value-arg.value); };
00053   AngPi operator*(orientation_t const &arg) const { return AngPi(value*arg); };
00054   AngPi operator/(orientation_t const &arg) const { return AngPi(value/arg); };
00055   
00056   AngPi& operator=(AngPi const &arg) { value = arg.value; return(*this); };
00057   AngPi& operator=(orientation_t const &arg) { value = arg; normalize(); return(*this); };
00058   AngPi& operator+=(orientation_t const &arg) { value += arg; normalize(); return(*this); };
00059   AngPi& operator-=(orientation_t const &arg) { value -= arg; normalize(); return(*this); };
00060   AngPi& operator*=(orientation_t const &arg) { value *= arg; normalize(); return(*this); };
00061   AngPi& operator/=(orientation_t const &arg) { value /= arg; normalize(); return(*this); };
00062   
00063   operator orientation_t() const { return value; }; //!< conversion operator for going back to the primitive type
00064   
00065 protected:
00066   void normalize(); //!< modifies #value to put it back in range
00067   orientation_t value; //!< holds the angle, should be kept normalized at all times
00068 };
00069 
00070 //! Angular distance: value is between 0 and pi/2
00071 AngPi angdist(AngPi const &arg1, AngPi const &arg2);
00072 
00073 //! Circular arithmetic on angles between 0 and two pi (360 degrees)
00074 class AngTwoPi {
00075   friend AngPi angdist(AngTwoPi const &arg1, AngTwoPi const &arg2);
00076 public:
00077   AngTwoPi(void) : value(0) {}; //!< constructor, #value defaults to 0
00078   AngTwoPi(direction_t const &v) : value(v) { normalize(); } //!< conversion operator allows implicit construction from primitive
00079   
00080   AngTwoPi operator+(AngTwoPi const &arg) const { return AngTwoPi(value+arg.value); };
00081   AngTwoPi operator-(AngTwoPi const &arg) const { return AngTwoPi(value-arg.value); };
00082   AngTwoPi operator*(direction_t const &arg) const { return AngTwoPi(value*arg); };
00083   AngTwoPi operator/(direction_t const &arg) const { return AngTwoPi(value/arg); };
00084   
00085   AngTwoPi& operator=(AngTwoPi const &arg) { value = arg.value; return(*this); };
00086   AngTwoPi& operator=(direction_t const &arg) { value = arg; normalize(); return(*this); };
00087   AngTwoPi& operator+=(direction_t const &arg) { value += arg; normalize(); return(*this); };
00088   AngTwoPi& operator-=(direction_t const &arg) { value -= arg; normalize(); return(*this); };
00089   AngTwoPi& operator*=(direction_t const &arg) { value *= arg; normalize(); return(*this); };
00090   AngTwoPi& operator/=(direction_t const &arg) { value /= arg; normalize(); return(*this); };
00091   
00092   operator direction_t() const { return value; }; //!< conversion operator for going back to the primitive type
00093   
00094 protected:
00095   void normalize(); //!< modifies #value to put it back in range
00096   direction_t value; //!< holds the angle, should be kept normalized at all times
00097 };
00098 
00099 //! Angular distance: value is between 0 and pi
00100 AngPi angdist(AngTwoPi const &arg1, AngTwoPi const &arg2);
00101 
00102 //! Circular arithmetic on angles between -pi and pi (360 degrees)
00103 class AngSignPi {
00104   friend AngPi angdist(AngSignPi const &arg1, AngSignPi const &arg2);
00105 public:
00106   AngSignPi(void) : value(0) {}; //!< constructor, #value defaults to 0
00107   AngSignPi(direction_t const &v) : value(v) { normalize(); } //!< conversion operator allows implicit construction from primitive
00108   
00109   AngSignPi operator+(AngSignPi const &arg) const { return AngSignPi(value+arg.value); };
00110   AngSignPi operator-(AngSignPi const &arg) const { return AngSignPi(value-arg.value); };
00111   AngSignPi operator*(direction_t const &arg) const { return AngSignPi(value*arg); };
00112   AngSignPi operator/(direction_t const &arg) const { return AngSignPi(value/arg); };
00113   
00114   AngSignPi& operator=(AngSignPi const &arg) { value = arg.value; return(*this); };
00115   AngSignPi& operator=(direction_t const &arg) { value = arg; normalize(); return(*this); };
00116   AngSignPi& operator+=(direction_t const &arg) { value += arg; normalize(); return(*this); };
00117   AngSignPi& operator-=(direction_t const &arg) { value -= arg; normalize(); return(*this); };
00118   AngSignPi& operator*=(direction_t const &arg) { value *= arg; normalize(); return(*this); };
00119   AngSignPi& operator/=(direction_t const &arg) { value /= arg; normalize(); return(*this); };
00120   
00121   operator direction_t() const { return value; }; //!< conversion operator for going back to the primitive type
00122 
00123 protected:
00124   void normalize(); //!< modifies #value to put it back in range
00125   direction_t value; //!< holds the angle, should be kept normalized at all times
00126 };
00127 
00128 //! Angular distance: value is between 0 and pi
00129 AngPi angdist(AngSignPi const &arg1, AngSignPi const &arg2);
00130 
00131 //! Turn with an explicit direction:  angles between -2*pi and 2*pi (+/- 360 degrees)
00132 class AngSignTwoPi {
00133 public:
00134   AngSignTwoPi(void) : value(0) {}; //!< constructor, #value defaults to 0
00135   AngSignTwoPi(direction_t const &v) : value(v) { normalize(); } //!< conversion operator allows implicit construction from primitive
00136   
00137   AngSignTwoPi operator+(AngSignTwoPi const &arg) const { return AngSignTwoPi(value+arg.value); };
00138   AngSignTwoPi operator-(AngSignTwoPi const &arg) const { return AngSignTwoPi(value-arg.value); };
00139   AngSignTwoPi operator*(direction_t const &arg) const { return AngSignTwoPi(value*arg); };
00140   AngSignTwoPi operator/(direction_t const &arg) const { return AngSignTwoPi(value/arg); };
00141   
00142   AngSignTwoPi& operator=(AngSignTwoPi const &arg) { value = arg.value; return(*this); };
00143   AngSignTwoPi& operator=(direction_t const &arg) { value = arg; normalize(); return(*this); };
00144   AngSignTwoPi& operator+=(direction_t const &arg) { value += arg; normalize(); return(*this); };
00145   AngSignTwoPi& operator-=(direction_t const &arg) { value -= arg; normalize(); return(*this); };
00146   AngSignTwoPi& operator*=(direction_t const &arg) { value *= arg; normalize(); return(*this); };
00147   AngSignTwoPi& operator/=(direction_t const &arg) { value /= arg; normalize(); return(*this); };
00148   
00149   operator direction_t() const { return value; }; //!< conversion operator for going back to the primitive type
00150 
00151 protected:
00152   void normalize(); //!< modifies #value to put it back in range
00153   direction_t value; //!< holds the angle, should be kept normalized at all times
00154 };
00155 
00156 #endif