HomgPoint3D.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 _HomgPoint3D_hh_
#define _HomgPoint3D_hh_
#include "bias_config.h" 

#include <Base/Debug/Error.hh>
#include <Base/Math/Vector4.hh>
#include <Base/Math/Vector3.hh>
#include <limits>

#define HOMGPOINT3D_TYPE double
#define HOMGPOINT3D_EPS 1E-12
#define HOMGPOINT3D_TYPE_INF std::numeric_limits<double>::infinity()

namespace BIAS {

/** @class HomgPoint3D
    @ingroup g_geometry
    @brief class HomgPoint3D describes a point with 3 degrees of 
    freedom in projective coordinates. 
        
    manual loop unrolling is used if possible. 
    The Vector is in row-major order (4 rows, 1 column)
    the indizes begin with zero (to size-1)
    
    constructors and assignment operators are never inherited, 
    so they have be wrapped (or reimplemented) here. 
    @author Jan Woetzel
    @status tested in part (03/04/2002)
    
    To DO:
    - CrossProduvt  von zwei HomgPOint3D 
    (Aunterscheidung beide w==0 / beide w!=0, sonst Meldung.
**/

  // all inline - nothing to compile JW
  class BIASGeometryBase_EXPORT HomgPoint3D : public Vector4<HOMGPOINT3D_TYPE> 
  {
  public:

    inline HomgPoint3D():Vector4 <HOMGPOINT3D_TYPE> () {};

    inline HomgPoint3D(const HomgPoint3D & p)
      : Vector4<HOMGPOINT3D_TYPE>(p) {};
    
    inline HomgPoint3D(const Vector4<HOMGPOINT3D_TYPE> & p) 
      : Vector4<HOMGPOINT3D_TYPE>(p) {};

    explicit inline HomgPoint3D(const Vector3<HOMGPOINT3D_TYPE>& vec);

    inline ~HomgPoint3D() {};

    /** assignment with a constant value for all elements
        @author Jan Woetzel
        @status alpha (03/01/2002) **/
     explicit inline HomgPoint3D(const HOMGPOINT3D_TYPE & scalar)
      : Vector4<HOMGPOINT3D_TYPE>(scalar) {};

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

    /** constructor with element assignment
        @author Jan Woetzel
        @status alpha (03/01/2002) **/
     explicit inline HomgPoint3D(char *s)
      : Vector4<HOMGPOINT3D_TYPE>(s) {};

    /** constructor using euclidic coordinates, setting the 4th 
        HomgPoint3D component to 1
        @author Ingo Thomsen
        @status untested */
    inline HomgPoint3D(const HOMGPOINT3D_TYPE & x,
                       const HOMGPOINT3D_TYPE & y,
                       const HOMGPOINT3D_TYPE & z);

    /** constructor using projective coordinates and initializes *this 
        from Vector<double> of length 4 or length 3 in this case the 
        fourth entry is 1.0.
        @author JMF
        @status tested */
    inline HomgPoint3D(const Vector<double>& vec);

    /** @author woelk 11/2003 */
    inline HomgPoint3D(const HOMGPOINT3D_TYPE & x,
                       const HOMGPOINT3D_TYPE & y,
                       const HOMGPOINT3D_TYPE & z,
                       const HOMGPOINT3D_TYPE & w)
      : Vector4<HOMGPOINT3D_TYPE>(x, y, z, w) {};

    inline bool IsAtInfinity() const;

    inline HOMGPOINT3D_TYPE GetX() const;
    inline HOMGPOINT3D_TYPE GetY() const;
    inline HOMGPOINT3D_TYPE GetZ() const;
    inline HOMGPOINT3D_TYPE GetW() const;
    inline HOMGPOINT3D_TYPE GetW();

    /** assignment operator
        set the elements of this HomgPoint to the element of vec
        @author Jan Woetzel
        @status alpha (02/25/2002)
    **/
    inline HomgPoint3D& operator=(const Vector4<HOMGPOINT3D_TYPE>& vec);

    /** assignment operator
        set the vector elementwise to scalar value
        @author Jan Woetzel
        @status alpha (02/28/2002)
    **/
    inline HomgPoint3D& operator=(const HOMGPOINT3D_TYPE& scalar);
    inline HomgPoint3D& operator=(const Vector3<HOMGPOINT3D_TYPE> &vec);

    /** homogenize class data member elements to W==1 by divison by W
        @author Jan Woetzel
        @status alpha (02/28/2002)**/
    inline void Homogenize();

    /** @author woelk 10 2003 */
    inline bool IsHomogenized() const {
      return data_[3]<1.0+HOMGPOINT3D_EPS && 
        data_[3]>1.0-HOMGPOINT3D_EPS;
    }

    /** set elementwise with given 3 euclidean scalar values.
        the additional W-coordinate is set to 1
        @author Jan Woetzel
        @status alpha (03/05/2002) **/
    inline void Set(const HOMGPOINT3D_TYPE& x, const HOMGPOINT3D_TYPE& y,
                    const HOMGPOINT3D_TYPE& z);

    /** set elementwise 
        @author woelk 11/2003 */
    inline void Set(const HOMGPOINT3D_TYPE& x, const HOMGPOINT3D_TYPE& y,
                    const HOMGPOINT3D_TYPE& z, const HOMGPOINT3D_TYPE& w)
    { Vector4<HOMGPOINT3D_TYPE>::Set(x, y, z, w); }

    /** set from Vector3
        @author woelk 03/2004 */
    inline void Set(const Vector3<HOMGPOINT3D_TYPE> &vec);

    /** calculate affine coordinates of this and write them to dest
        affine coordinates are projective coordinates with last element w == 1;
        For w==0 the first three elements are copied describing a direction,
        not a point.**/
    inline void GetEuclidean(Vector3<HOMGPOINT3D_TYPE> &dest) const;
    inline Vector3<HOMGPOINT3D_TYPE> GetEuclidean() const;
    /// deprecated versions
    inline void GetEuclidian(Vector3<HOMGPOINT3D_TYPE> &dest) const {
      this->GetEuclidean(dest);
    };
    inline Vector3<HOMGPOINT3D_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 HOMGPOINT3D_TYPE GetEuclideanX() const;
    inline HOMGPOINT3D_TYPE GetEuclideanY() const;
    inline HOMGPOINT3D_TYPE GetEuclideanZ() const;
    /// deprecated versions
    inline HOMGPOINT3D_TYPE GetEuclidianX() const {
      return this->GetEuclideanX();
    };
    inline HOMGPOINT3D_TYPE GetEuclidianY() const {
      return this->GetEuclideanY();
    };
    inline HOMGPOINT3D_TYPE GetEuclidianZ() const {
      return this->GetEuclideanZ();
    };

    /** Comparison operator 'equal'
        @author Arne Petersen
        @status untested **/
    inline bool operator==(const HomgPoint3D& arg) const;

    /** Comparison operator 'not equal'
        @author Ingo Thomsen
        @status tested **/
    inline bool operator!=(const HomgPoint3D& arg) const;

    inline HOMGPOINT3D_TYPE Distance(const HomgPoint3D& point) const;

  }; // class




  inline HomgPoint3D::HomgPoint3D(const Vector3<HOMGPOINT3D_TYPE>& vec) 
  {
    Set(vec);
  }

  inline HomgPoint3D::HomgPoint3D(const HOMGPOINT3D_TYPE & x,
                                  const HOMGPOINT3D_TYPE & y,
                                  const HOMGPOINT3D_TYPE & z)
  {
    data_[0] = x;
    data_[1] = y;
    data_[2] = z;
    data_[3] = 1;
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetX() const
  {
    return data_[0];
  }
  
  inline HOMGPOINT3D_TYPE HomgPoint3D::GetY() const
  {
    return data_[1];
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetZ() const
  {
    return data_[2];
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetW()
  {
    if (data_[3] > -HOMGPOINT3D_EPS && data_[3] < HOMGPOINT3D_EPS)
      data_[3] = 0.0;
    return data_[3];
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetW() const
  {
    if (data_[3] > -HOMGPOINT3D_EPS && data_[3] < HOMGPOINT3D_EPS)
      return 0.0;
    else
      return data_[3];
  }

  inline HomgPoint3D& 
  HomgPoint3D::operator=(const Vector3<HOMGPOINT3D_TYPE> &vec)
  {
    data_[0] = vec[0];
    data_[1] = vec[1];
    data_[2] = vec[2];
    data_[3] = 1.0;
    return *this;
  }

  inline HomgPoint3D& 
  HomgPoint3D::operator=(const Vector4<HOMGPOINT3D_TYPE> &vec)
  {
    Copy(vec.GetData());
    return *this;
  }

  inline HomgPoint3D& 
  HomgPoint3D::operator=(const HOMGPOINT3D_TYPE& scalar) 
  {
    data_[0] = scalar;
    data_[1] = scalar;
    data_[2] = scalar;
    data_[3] = scalar;
    return *this;
  }

  inline HomgPoint3D::HomgPoint3D(const Vector<double>& vec){
    
    switch (vec.size()){
    case 3:
      data_[3]=1.0;
    case 4:
      for (int i=0;i<vec.size();i++)
        data_[i]=vec[i];
      break;
    default:
      BIASERR("Initialization of  HomgPoint3D from Vector with length"
             << "!= 3 or !=4,length is " << vec.size());
    }
    
  }

  inline void HomgPoint3D::Homogenize() 
  {
    HOMGPOINT3D_TYPE w = GetW();
    if (fabs(w) > HOMGPOINT3D_EPS){
      data_[0] = data_[0] / w;
      data_[1] = data_[1] / w;
      data_[2] = data_[2] / w;
      data_[3] = data_[3] / w;
    } else {
      BEXCEPTION("cannot homogenize, w is zero: "<<w);
    }
  }

  inline void HomgPoint3D::Set(const HOMGPOINT3D_TYPE & x, 
                               const HOMGPOINT3D_TYPE & y,
                               const HOMGPOINT3D_TYPE & z) 
  {
    Vector4<HOMGPOINT3D_TYPE>::Set(x, y, z, 1);
  }

  inline void HomgPoint3D::Set(const Vector3<HOMGPOINT3D_TYPE>& vec)
  {
    data_[0] = vec[0];
    data_[1] = vec[1];
    data_[2] = vec[2];
    data_[3] = 1.0;
  }
  
  inline void HomgPoint3D::GetEuclidean(Vector3<HOMGPOINT3D_TYPE> &dest) const
  {
    HOMGPOINT3D_TYPE w = GetW();
    if (w != 0.0){
      dest[0] = data_[0] / w;
      dest[1] = data_[1] / w;
      dest[2] = data_[2] / w;
    } else {
      dest[0] = data_[0];
      dest[1] = data_[1];
      dest[2] = data_[2];
    } 
  }

  inline Vector3<HOMGPOINT3D_TYPE> HomgPoint3D::GetEuclidean() const
  {
    Vector3<HOMGPOINT3D_TYPE> dest;
    this->GetEuclidean(dest);
    return dest;
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetEuclideanX() const
  {
    HOMGPOINT3D_TYPE w = GetW();
    BIASASSERT(w != 0.0);
    return (data_[0] / w);
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetEuclideanY() const
  {
    HOMGPOINT3D_TYPE w = GetW();
    BIASASSERT(w != 0.0);
    return (data_[1] / w);
  }

  inline HOMGPOINT3D_TYPE HomgPoint3D::GetEuclideanZ() const
  {
    HOMGPOINT3D_TYPE w = GetW();
    BIASASSERT(w != 0.0);
    return (data_[2] / w);
  }

  inline bool HomgPoint3D::operator==(const HomgPoint3D& arg) const
  {
    //check 'homogenized' equality
    bool res =   (data_[0]*arg[3] == data_[3]*arg[0])
               &&(data_[1]*arg[3] == data_[3]*arg[1])
               &&(data_[2]*arg[3] == data_[3]*arg[2]);
    //return 'homogenized' equality if at least one point is not at infinity
    if ((data_[3]!=0.0)||(arg[3]!=0.0)) return res;
    //if both points are at infinity, check if they're parallel
    res =    (data_[2]*arg[1] == data_[1]*arg[2])
          && (data_[2]*arg[0] == data_[0]*arg[2])
          && (data_[1]*arg[0] == data_[0]*arg[1]);
    return res;
  }
  
  inline bool HomgPoint3D::operator!=(const HomgPoint3D& arg) const
  {
    return !(*this == arg);
  }

  inline bool HomgPoint3D::IsAtInfinity() const
  {
    return (data_[3] > -HOMGPOINT3D_EPS && data_[3] < HOMGPOINT3D_EPS);
  }

  inline HOMGPOINT3D_TYPE 
  HomgPoint3D::Distance(const HomgPoint3D& point) const
  {
    if ((data_[3] > -HOMGPOINT3D_EPS && data_[3] < HOMGPOINT3D_EPS) ||
        (point[3] > -HOMGPOINT3D_EPS && point[3] < HOMGPOINT3D_EPS)) {
      return HOMGPOINT3D_TYPE_INF;
    }
    HOMGPOINT3D_TYPE dx=data_[0]/data_[3] - point[0]/point[3];
    HOMGPOINT3D_TYPE dy=data_[1]/data_[3] - point[1]/point[3];
    HOMGPOINT3D_TYPE dz=data_[2]/data_[3] - point[2]/point[3];
    return sqrt(dx*dx + dy*dy + dz*dz);
  }

} // namespace BIAS
#endif