HomgPoint2D.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#ifndef _HomgPoint2D_hh_
#define _HomgPoint2D_hh_
#include "bias_config.h" 

#ifdef WIN32
// implement non-standard (double)rint(double x)
#  define rint(x) (floor(x+0.5))
#endif //_WIN32

#include <Base/Debug/Error.hh>

#include <Base/Math/Vector3.hh>
#include <Base/Math/Vector.hh>
#include <Base/Math/Vector2.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <limits>
#include <cmath>

                         // the type of each element:
#define HOMGPOINT2D_TYPE double
                         // values whose abs is < epsilon are handled as zero
#define HOMGPOINT2D_TYPE_EPS 1E-12
                         // the infinity value
#define HOMGPOINT2D_TYPE_INF std::numeric_limits<double>::infinity()

namespace BIAS {
  /**
     @class HomgPoint2D
     @ingroup g_geometry
     @brief class HomgPoint2D describes a point with 2 degrees of freedom in 
     projective coordinates. 
    
     manual loop unrolling is used if possible. 
     The Vector is in row-major order (3 rows, 1 column)
     the indizes begin with zero (to size-1)
    
     constructors and assignment operators are never inherited, so they are 
     wrapped (or reimplemented) here. 
     @author Jan Woetzel
     @status untested  (03/04/2002) **/
  class BIASGeometryBase_EXPORT HomgPoint2D : public Vector3<HOMGPOINT2D_TYPE> 
  {
  public:
    enum ELocation { LOC_LEFT, LOC_RIGHT, LOC_MIDDLE, LOC_UPPER, 
                     LOC_LOWER, LOC_undef};

    enum EPosition { Left, Right, Bottom, Top, TopLeft, TopRight, BottomLeft, 
                     BottomRight, Center, Infinity, InvalidPosition };
  
    /** default constructor
        @author Jan Woetzel
        @status alpha (02/28/2002) **/
    inline HomgPoint2D() 
      : Vector3 < HOMGPOINT2D_TYPE > () {};

    /**  constructor
         @author JMF
    **/
    // inline HomgPoint2D(const Vector3 < HOMGPOINT2D_TYPE >& p) 
    // : Vector3 < HOMGPOINT2D_TYPE > (p) {};

    /** copy constructor
        @author Jan Woetzel (03/05/2002) **/
    inline HomgPoint2D(const HomgPoint2D & p) 
      : Vector3 <HOMGPOINT2D_TYPE> (p) {};


    /** assignment with a constant value for all elements
        @author Jan Woetzel (03/01/2002) **/
    explicit inline HomgPoint2D(const HOMGPOINT2D_TYPE & scalar)
      :Vector3 <HOMGPOINT2D_TYPE> (scalar) {};

    /** assignment with an array of values
        which is copied into this ones class members
        @author Jan Woetzel (02/28/2002) **/
    explicit inline HomgPoint2D(const HOMGPOINT2D_TYPE * pv)
      :Vector3 <HOMGPOINT2D_TYPE> (pv) {};

    /** constructor with element assignment
        @author Jan Woetzel (03/01/2002) **/
    explicit inline HomgPoint2D(char *s):Vector3 <HOMGPOINT2D_TYPE> (s) {};

    /** constructor from Vector3    */
    inline HomgPoint2D(const Vector3<double>& vec) {
      data_[0] = vec[0];
      data_[1] = vec[1];
      data_[2] = vec[2];
    };

    /** constructor 
        @author streckel 07 2006 **/
    explicit inline HomgPoint2D(const Vector2<HOMGPOINT2D_TYPE>& vec) {
      Set(vec);
    }

    /** constructor using euclidic coordinates, setting the 3rd HomgPoint2D 
        component to 1
        @author Ingo Thomsen **/
    inline HomgPoint2D(const HOMGPOINT2D_TYPE & x,
                       const HOMGPOINT2D_TYPE & y) 
    {
      data_[0] = x;
      data_[1] = y;
      data_[2] = 1;
    };
    
    /** constructor 
        @author woelk 02 2003 **/
    inline HomgPoint2D(const HOMGPOINT2D_TYPE & x,
                       const HOMGPOINT2D_TYPE & y,
                       const HOMGPOINT2D_TYPE & w) 
    { data_[0] = x; data_[1] = y; data_[2] = w; };
    
    inline HOMGPOINT2D_TYPE GetW() const { return data_[2]; };

    /** destructor
        @author Jan Woetzel (02/28/2002)**/
    ~HomgPoint2D() {};

    /** assignment operator
        set the elements of this HomgPoint to the element of vec
        @author Jan Woetzel
        @status alpha (02/25/2002)**/
    inline HomgPoint2D& operator=(const Vector3<HOMGPOINT2D_TYPE> &vec) {
      Vector3<HOMGPOINT2D_TYPE>::Set(vec.GetData());
      return *this;
    };

    /** assignment operator
        set the vector elementwise to scalar value
        @author Jan Woetzel
        @status alpha (02/28/2002)**/
    inline HomgPoint2D & operator= (const HOMGPOINT2D_TYPE & scalar) {
      Vector3 < HOMGPOINT2D_TYPE >::Set(scalar);
      return *this;
    };

    
    inline bool IsAtInfinity() const
    { return (data_[2]>-HOMGPOINT2D_TYPE_EPS && 
              data_[2]<HOMGPOINT2D_TYPE_EPS); };


    /** set elementwise with given 2 euclidean scalar values.
        the additional W-coordinate is set to 1
        @author Jan Woetzel
        @status alpha (03/05/2002) **/
    inline void Set(const HOMGPOINT2D_TYPE & x, const HOMGPOINT2D_TYPE & y) 
    { Vector3<HOMGPOINT2D_TYPE>::Set(x, y, 1.0); };

    inline void Set(const HOMGPOINT2D_TYPE& x, const HOMGPOINT2D_TYPE& y,
                    const HOMGPOINT2D_TYPE& z) 
    { Vector3<HOMGPOINT2D_TYPE>::Set(x, y, z); };

    /** Sets this form elements of Vector<T>. */
    inline void Set(const Vector<HOMGPOINT2D_TYPE> &vec) {
      if (vec.size() != 3){
        BIASERR("The vector for initialization of Vector3 has not length 3");
      } else { 
        data_[0] = vec[0];
        data_[1] = vec[1];
        data_[2] = vec[2];
      }
    }
    
    /** set from Vector2 
        @author streckel 07 2006 **/
    inline void Set(const Vector2<HOMGPOINT2D_TYPE>& vec)
    {
      data_[0] = vec[0];
      data_[1] = vec[1];
      data_[2] = 1.0;
    }

    /** @author woelk 04 2003 */
    inline bool IsHomogenized() const {
      return data_[2]<1+HOMGPOINT2D_TYPE_EPS && 
        data_[2]>1-HOMGPOINT2D_TYPE_EPS;
    }

    /** @author esquivel */
    inline bool IsNormalized() const {
      return Equal(NormL2(), 1.0, HOMGPOINT2D_TYPE_EPS);
    }

    /** homogenize class data member elements to W==1 by divison by W
        @author koeser (originally jw)
        @status optimized **/
    inline HomgPoint2D& Homogenize() 
    {
      register const HOMGPOINT2D_TYPE w = data_[2];
      if (data_[2]>-HOMGPOINT2D_TYPE_EPS && 
          data_[2]<HOMGPOINT2D_TYPE_EPS) {
        // point is at infinity
        data_[2] = 0.0;
      } else {
        // w = data_[3]  is not in [-eps..eps]  (means non-zero)
        data_[0] /= w;
        data_[1] /= w;
        data_[2]  = 1.0;
      }
      return (*this);
    };

    /** homogenizes the point and transforms the associated covariance matrix
        @author woelk 03/2005 
        @status untested */
    HomgPoint2D& Homogenize(Matrix3x3<double>& cov);
 

    /** calculate affine coordinates of this and write them to dest
        affine coordinates are projective coordinates with last element w == 1;
        Only possible for w !=0 because w==0 describes points of infinity 
        which are not in euclidean space (but may be interpreted as 
        directions)
        @author Jan Woetzel, (03/04/2002)
    */
    inline void GetEuclidean(Vector2 < HOMGPOINT2D_TYPE > &dest) const 
    {
      dest.GetData()[0] = GetEuclideanX();  // X
      dest.GetData()[1] = GetEuclideanY();  // Y
    };
    
    /** @brief return the euclidean coordinates of this homg 2D point
        Only possible for w !=0 because w==0 describes points of infinity 
        which are not in euclidean space (but may be interpreted 
        as directions)
        @author Jan Woetzel
        @status untested (03/04/2002)**/
    inline Vector2<HOMGPOINT2D_TYPE> GetEuclidean() const {
      Vector2<HOMGPOINT2D_TYPE> dest;
      this->GetEuclidean(dest);
      return dest;
    };

    // deprecated versions
    inline void GetEuclidian(Vector2 < HOMGPOINT2D_TYPE > &dest) const {
      this->GetEuclidean(dest);
    };
    inline Vector2<HOMGPOINT2D_TYPE> GetEuclidian() const {
      return this->GetEuclidean();
    };
    
    /** return the euclidean coordinates
        Only possible for w !=0 because w==0 describes points of 
        infinity which are not in euclidean space (but may be interpreted 
        as directions)
        @author Jan Woetzel
        @status untested (03/04/2002)**/
    inline HOMGPOINT2D_TYPE GetEuclideanX() const {
      BIASASSERT(!((GetW() > -HOMGPOINT2D_TYPE_EPS) && 
                   (GetW() < HOMGPOINT2D_TYPE_EPS)));   
      // test if w is zero (in eps env.)
      return ((*this)[0] / GetW());
    };
    
    inline HOMGPOINT2D_TYPE GetEuclideanY() const {
      BIASASSERT(!((GetW() > -HOMGPOINT2D_TYPE_EPS) && 
                   (GetW() < HOMGPOINT2D_TYPE_EPS)));   
      // test if w is zero (in eps env.)
      return ((*this)[1] / GetW());
    };

    // deprecated versions
    inline HOMGPOINT2D_TYPE GetEuclidianX() const {
      return this->GetEuclideanX();
    };
    inline HOMGPOINT2D_TYPE GetEuclidianY() const {
      return this->GetEuclideanY();
    };

    inline HOMGPOINT2D_TYPE Distance(const HomgPoint2D& point) const;
    inline HOMGPOINT2D_TYPE DistanceSquared(const HomgPoint2D& point) const;

    ////////////////////////////////////////////////////////////////////////
    ///////////////// Comparison operators /////////////////////////////////
    ////////////////////////////////////////////////////////////////////////

    /** Comparison operator 'equal'
        @author Arne Petersen
        @status untested**/
    inline bool operator==(const HomgPoint2D& arg) const {
      //Two homogeneos 2D-points are euqal if and only if the cross-product
      //of their 3D-coordinates is zero (the rays are colinear)
      return ((data_[2]*arg[1] == data_[1]*arg[2])
              && (data_[2]*arg[0] == data_[0]*arg[2])
              && (data_[1]*arg[0] == data_[0]*arg[1]));
    }


    /** Comparison operator 'not equal'
        @author Ingo Thomsen
        @status tested **/
    inline bool operator!= (const HomgPoint2D& arg) const
    { return !(*this == arg); };
    
    /** return image coordinates for drawing , coo[0]=x, coo[1]=y */
    inline bool GetImageCoo(unsigned int width, unsigned int height,
                            unsigned int coo[2]);
    
    inline enum EPosition
      DeterminePosition(unsigned int width, unsigned int height);

    /** @brief give point position relative to image borders
        the parameters minx=0, miny=0, maxx=width-1, maxy=height-1
        represent a typical image */
    inline enum EPosition
      DeterminePosition(const double& minx, const double& miny, 
                        const double& maxx, const double& maxy);



    /** @brief determines if point is within rectangular RegionOfInterest
        @attention point is assumed to be homogenized !
        @author koeser 03/2004 */
    inline bool IsInROIHomogenized(const HOMGPOINT2D_TYPE& xmin, 
                                   const HOMGPOINT2D_TYPE& ymin,
                                   const HOMGPOINT2D_TYPE& xmax, 
                                   const HOMGPOINT2D_TYPE& ymax) const;

    /** @brief determines if point is within rectangular RegionOfInterest
        @author koeser 03/2004 */
    inline bool IsInROI(const HOMGPOINT2D_TYPE& xmin, 
                        const HOMGPOINT2D_TYPE& ymin,
                        const HOMGPOINT2D_TYPE& xmax, 
                        const HOMGPOINT2D_TYPE& ymax) const;

  };    
  
  BIASGeometryBase_EXPORT std::ostream& operator<<(std::ostream& os, 
                                                   const enum HomgPoint2D::EPosition pos);

  // std::ostream& operator<<(std::ostream& os, const BIAS::HomgPoint2D p2D);

  //////////////////////////////////////////////////////////////////
  // implementations
  /////////////////////////////////////////////////////////////////


  
  inline HOMGPOINT2D_TYPE HomgPoint2D::Distance(const HomgPoint2D& point) const
  {
    if ((data_[2]>-HOMGPOINT2D_TYPE_EPS && data_[2]<HOMGPOINT2D_TYPE_EPS) ||
        (point[2]>-HOMGPOINT2D_TYPE_EPS && point[2]<HOMGPOINT2D_TYPE_EPS)) {
      return HOMGPOINT2D_TYPE_INF;
    }
    return sqrt(pow(data_[0]/data_[2] - point[0]/point[2],2) +
                pow(data_[1]/data_[2] - point[1]/point[2],2));
  }

  inline HOMGPOINT2D_TYPE HomgPoint2D::DistanceSquared(const HomgPoint2D& point) const
  {
    if ((data_[2]>-HOMGPOINT2D_TYPE_EPS && data_[2]<HOMGPOINT2D_TYPE_EPS) ||
        (point[2]>-HOMGPOINT2D_TYPE_EPS && point[2]<HOMGPOINT2D_TYPE_EPS)) {
      return HOMGPOINT2D_TYPE_INF;
    }
    return pow(data_[0]/data_[2] - point[0]/point[2],2) +
      pow(data_[1]/data_[2] - point[1]/point[2],2);
  }

  inline bool
  HomgPoint2D::GetImageCoo(unsigned int width, unsigned int height,
                           unsigned int coo[2])
  {
    bool result = false;
    Homogenize();
    if (data_[0] >= 0.0 && data_[0] < (width -1) &&
        data_[1] >= 0.0 && data_[1] < (height-1)){
      result = true;
      coo[0] = (unsigned int)rint(data_[0]);
      coo[1] = (unsigned int)rint(data_[1]);
    }
    return result;
  }

  inline enum HomgPoint2D::EPosition
  HomgPoint2D::DeterminePosition(unsigned int width, unsigned int height) {
    return DeterminePosition(0.0, 0.0, double(width-1), double(height-1));
  }

  inline enum HomgPoint2D::EPosition
  HomgPoint2D::DeterminePosition(const double& minx, const double& miny,
                                 const double& maxx, const double& maxy)
  {
    enum HomgPoint2D::EPosition pos = InvalidPosition;
    enum HomgPoint2D::ELocation xpos = LOC_undef, ypos = LOC_undef;
    
    if (IsAtInfinity()){
      //BIASERR("point at infinity");
      return pos;
    } else {
      (*this)[0]/=(*this)[2];
      (*this)[1]/=(*this)[2];
    }

    if ((*this)[0] < minx) {
      xpos = LOC_LEFT;
    } else if ((*this)[0] > maxx) {
      xpos = LOC_RIGHT;
    } else xpos = LOC_MIDDLE;

    if ((*this)[1] < miny) {
      ypos = LOC_UPPER;
    } else if ((*this)[1] > maxy) {
      ypos = LOC_LOWER;
    } else ypos = LOC_MIDDLE;

    switch (xpos) {
    case LOC_LEFT:
      switch (ypos) {
      case LOC_LOWER:
        pos = BottomLeft;
        break;
      case LOC_UPPER:
        pos = TopLeft;
        break;
      case LOC_MIDDLE:
        pos = Left;
        break;
      default:
        BIASERR("no valid ypos");
        break;
      }
      break;

    case LOC_MIDDLE:
      switch (ypos) {
      case LOC_LOWER:
        pos = Bottom;
        break;
      case LOC_UPPER:
        pos = Top;
        break;
      case LOC_MIDDLE:
        pos = Center;
        break;
      default:
        BIASERR("no valid ypos");
        break;
      }
      break;

    case LOC_RIGHT:
      switch (ypos) {
      case LOC_LOWER:
        pos = BottomRight;
        break;
      case LOC_UPPER:
        pos = TopRight;
        break;
      case LOC_MIDDLE:
        pos = Right;
        break;
      default:
        BIASERR("no valid ypos");
        break;
      }
      break;

    default:
      BIASERR("no valid xpos");
      break;
    }
    return pos;
  }

  inline bool 
  HomgPoint2D::IsInROIHomogenized(const HOMGPOINT2D_TYPE& xmin, 
                                  const HOMGPOINT2D_TYPE& ymin,
                                  const HOMGPOINT2D_TYPE& xmax, 
                                  const HOMGPOINT2D_TYPE& ymax) const {
#ifdef BIAS_DEBUG
    if (!IsHomogenized()) {
      BIASERR("Supplied unhomogenized point to IsInROIHomogenized(). "
              <<"Lucky you! Without BIAS_DEBUG enabled, you do not get this "
              <<"warning and there would be a really wrong result here! "
              <<"Fix your code and try again. Goodbye.");
      BIASABORT;
    }
#endif
    return (data_[0]>=xmin) && (data_[0]<=xmax) && 
      (data_[1]>ymin) && (data_[1]<ymax);
  }

  // this is code duplication, but for the sake of speed, it is done this way
  inline bool HomgPoint2D::IsInROI(const HOMGPOINT2D_TYPE& xmin, 
                                   const HOMGPOINT2D_TYPE& ymin,
                                   const HOMGPOINT2D_TYPE& xmax, 
                                   const HOMGPOINT2D_TYPE& ymax) const {
    return (data_[0]/data_[2]>=xmin) && (data_[0]/data_[2]<=xmax) && 
      (data_[1]/data_[2]>ymin) && (data_[1]/data_[2]<ymax);
  }


} // namespace BIAS
#endif