HomgLine3D.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 __HOMGLINE3D_HH__
#define __HOMGLINE3D_HH__
#include "bias_config.h" 

#include "HomgPoint3D.hh"

#define HOMGLINE3D_TYPE double
#define HOMGLINE3D_EPS 1e-12
namespace BIAS{

  /**
     @class HomgLine3D
     a homogenous homogenized line is described by a direction vector PointInf_
     (a point at infinity) and a starting point PointFin_ (which is the
     closest vector from the coordinate origin to the line
     if the line is not homogenized, it can bve described by any two points
  */
  class BIASGeometryBase_EXPORT HomgLine3D 
  {
  public:
    inline HomgLine3D();

    inline HomgLine3D(const HomgPoint3D& p1, const HomgPoint3D& p2);

    inline ~HomgLine3D();
    
    inline HomgPoint3D GetPointInf() const
    { return PointInf_; };

    inline HomgPoint3D GetPointFin() const
    { return PointFin_; };

    inline void Set(const HomgPoint3D& p1, const HomgPoint3D& p2);

    inline void Homogenize();

    void CalculateFootpoint(HomgPoint3D& p, HomgPoint3D& footp);

    inline void Distance(HomgPoint3D& p, HOMGLINE3D_TYPE& dist);
   
  protected:
    HomgPoint3D PointInf_, PointFin_;
  };



  inline HomgLine3D::HomgLine3D() : PointInf_(), PointFin_()
  {}
  
  inline HomgLine3D::HomgLine3D(const HomgPoint3D& p1, const HomgPoint3D& p2)
    : PointInf_(p1), PointFin_(p2)
  {}

  inline HomgLine3D::~HomgLine3D()
  {}

  inline void HomgLine3D::Set(const HomgPoint3D& p1, const HomgPoint3D& p2)
  {
    PointInf_ = p1;
    PointFin_ = p2;
  }

  inline void HomgLine3D::Homogenize()
  {
    Vector3<double> vi, v;
    PointInf_.Homogenize();
    PointFin_.Homogenize();
    
    // assume one point in euklidean space
    BIASASSERT(PointInf_.GetW() != 0.0 || PointFin_.GetW() != 0.0);
    
    if (PointFin_.GetW() == 0){ // points are swapped
      HomgPoint3D tmp = PointFin_;
      PointFin_ = PointInf_;
      PointInf_ = tmp;
    }

    if (PointInf_.GetW() != 0.0){ // move PointInf_ to infinity
      PointInf_ = PointInf_ - PointFin_;
      // norm2 can be used because w == 0
      PointInf_ /= PointInf_.NormL2();
    } 

    // calculate footpoint to coordinate origin on line
    vi[0] = PointInf_[0];
    vi[1] = PointInf_[1];
    vi[2] = PointInf_[2];
    v[0] = PointFin_[0];
    v[1] = PointFin_[1];
    v[2] = PointFin_[2];
    double t;
    v.ScalarProduct(vi, t);
    v = v - (vi * t);
    PointFin_[0] = v[0];
    PointFin_[1] = v[1];
    PointFin_[2] = v[2];
    PointFin_[3] = 1.0;
  }


  inline void HomgLine3D::CalculateFootpoint(HomgPoint3D& p, 
                                             HomgPoint3D& footp)
  {
    Vector3<double> vi, v, tmp;

    Homogenize();
    // cannot calculate footpoint to infinite point
    BIASASSERT(p.GetW() !=0.0);
    vi[0] = PointInf_[0];
    vi[1] = PointInf_[1];
    vi[2] = PointInf_[2];
    v[0] = PointFin_[0];
    v[1] = PointFin_[1];
    v[2] = PointFin_[2];
    tmp[0] = p[0] / p.GetW();
    tmp[1] = p[1] / p.GetW();
    tmp[2] = p[2] / p.GetW();
    double t;
    tmp.SubIP(v);
    vi.ScalarProduct(v, t);
    v = v + (vi * t);
    footp[0] = v[0];
    footp[1] = v[1];
    footp[2] = v[2];
    footp[3] = 1.0;
  }

  inline void 
  HomgLine3D::Distance(HomgPoint3D& p, HOMGLINE3D_TYPE& dist)
  {
    HomgPoint3D footp;
    CalculateFootpoint(p, footp);
    footp -= p;
    dist = footp.NormL2();
  }
  
} // namespace BIAS


#endif // __HOMGLINE3D_HH__