HomgPlane3D.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 __HOMGPLANE3D_HH__
#define __HOMGPLANE3D_HH__
#include "bias_config.h" 

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

#include "HomgPoint3D.hh"

#define HOMGPLANE3D_TYPE double
// values whose abs is < epsilon are handled as zero
#define HOMGPLANE3D_EPS 1E-12

namespace BIAS {
  /** @class HomgPlane3D
      @ingroup g_geometry
      @brief A homogeneous plane (in P^3)
      All points X on the plane p fulfill p ' * X = 0 */
  class BIASGeometryBase_EXPORT HomgPlane3D 
    : public Vector4<HOMGPLANE3D_TYPE>
  {
  public:
    inline HomgPlane3D()
    { data_[0] = data_[1] = data_[3] = 0.0 ; data_[2] = 1.0 ; };
    
    /** @brief set four values of the plane vector */
    inline HomgPlane3D(HOMGPLANE3D_TYPE a, HOMGPLANE3D_TYPE b,
                       HOMGPLANE3D_TYPE c, HOMGPLANE3D_TYPE d)
    { data_[0]=a; data_[1]=b; data_[2]=c; data_[3]=d; };

    /** @brief constructs a homogeneous plane from three points 
        @author koeser 07/2004 */
    inline HomgPlane3D(const HomgPoint3D& p1,
                       const HomgPoint3D& p2,
                       const HomgPoint3D& p3) {
      SetFromPoints(p1, p2, p3);
    }

    /** @brief construct plane, given arbitrary point and normal
        @param NormalVector unit length normal vector    
        @param PointOnPlane arbitrary point on the plane
        @author koeser 02/2005  */
    explicit inline HomgPlane3D(const Vector3<double>& PointOnPlane, 
                                const  Vector3<double>& NormalVector) {
      Set(PointOnPlane, NormalVector); }

    explicit inline HomgPlane3D(const HomgPoint3D& PointOnPlane, 
                                const Vector3<double>& NormalVector) {
      Set(PointOnPlane, NormalVector); }

    explicit inline HomgPlane3D(const Vector4<HOMGPLANE3D_TYPE>& theplane)
    { Vector4<HOMGPLANE3D_TYPE>::operator=(theplane); };

    explicit inline HomgPlane3D(const Vector<HOMGPLANE3D_TYPE>& theplane)
    { SetFromVector(theplane); };

    /** @brief constructs a homogeneous plane from three points 
        @author koeser 07/2004 */
    inline void SetFromPoints(const HomgPoint3D& p1,
                              const HomgPoint3D& p2,
                              const HomgPoint3D& p3);
      
    inline void SetFromVector(const Vector<HOMGPLANE3D_TYPE>& theplane) {
      BIASASSERT(theplane.size()==4);
      data_[0]=theplane[0]; data_[1]=theplane[1]; 
      data_[2]=theplane[2]; data_[3]=theplane[3];
    }

    /** @brief empty destructor */
    inline ~HomgPlane3D() {};

    /** @brief construct plane, given arbitrary point and normal
        @param NormalVector unit length normal vector    
        @param PointOnPlane arbitrary point on the plane
        @author koeser 02/2005  */
    void Set(const Vector3<double>& PointOnPlane, 
             const Vector3<double>& NormalVector) {
       data_[0] = NormalVector[0]; 
       data_[1] = NormalVector[1]; 
       data_[2] = NormalVector[2]; 
       data_[3] = -1.0 * NormalVector.ScalarProduct(PointOnPlane);
    }

    /** @brief construct plane, given arbitrary point and normal
        @param NormalVector normal vector with length != 0
        @param PointOnPlane arbitrary point on the plane (also at infinity)
        @author woelk 06/2008 (c) www.vision-n.de  */
    void Set(const HomgPoint3D& PointOnPlane, 
             const Vector3<double>& NormalVector);

    /** @brief set first three components to vector of norm 1 and scales
        w accordingly => encodes Hesse normal form 

        afterwards w contains the distance of the plane
        from the origin, while the first three coordinates are the normal
        vector in euclidean 3-space. This is called the Hesse normal form. */
    inline void Normalize();

    /** @brief  returns normal vector */
    inline void GetNormalVector(Vector3<HOMGPLANE3D_TYPE>& normalv) const;
    
    /** @brief returns normal vector, only wrapper */
    inline Vector3<HOMGPLANE3D_TYPE> GetNormalVector() const;
    
    /** @brief calculates squared distance of a point from plane */
    inline HOMGPLANE3D_TYPE DistanceSquared(const HomgPoint3D& point) const;
    
    /** @brief compute the intersection point of the plane with a line (defined
        by point and direction)

        @param PointOnLine one arbitrary point on the line
        @param LineDir The direction of the line, LineDir[3]==0 !
        @param Intersection (output) A point lying on the line and on the plane
        @return - 0 on success
                - +1 point is at infinity (line and plane parallel)
                - -1 line is in the plane, whole line is solution
        @author koeser 01/2005 */
    inline int GetLineIntersection(const HomgPoint3D& PointOnLine,
                                   const HomgPoint3D& LineDir,
                                   HomgPoint3D& Intersection) const;

    /** @brief finds the closest point which is on the plane
        @author koeser */
    inline HomgPoint3D ProjectPoint(const HomgPoint3D& origPoint) const;

  }; // class


  // ----------------------- inline implementation ---------------------------

  inline void HomgPlane3D::Normalize() {
    (*this) /= sqrt(data_[0]*data_[0]+data_[1]*data_[1]+data_[2]*data_[2]);
  }

  inline Vector3<HOMGPLANE3D_TYPE>
  HomgPlane3D::GetNormalVector() const
  {
    HOMGPLANE3D_TYPE norm = 
      sqrt(data_[0]*data_[0]+data_[1]*data_[1]+data_[2]*data_[2]);
    Vector3<HOMGPLANE3D_TYPE> normal(data_[0] / norm, data_[1] / norm, 
                     data_[2] / norm);
    return normal;
  }

  inline void 
  HomgPlane3D::GetNormalVector(Vector3<HOMGPLANE3D_TYPE>& normalv) const
  {
    HOMGPLANE3D_TYPE norm = 
      sqrt(data_[0]*data_[0]+data_[1]*data_[1]+data_[2]*data_[2]);
    normalv[0] = data_[0] / norm;
    normalv[1] = data_[1] / norm;
    normalv[2] = data_[2] / norm;
  }


  inline HOMGPLANE3D_TYPE
  HomgPlane3D::DistanceSquared(const HomgPoint3D& point) const
  {
    HOMGPLANE3D_TYPE dist = data_[0] * point[0] + data_[1] * point[1] + 
      data_[2] * point[2] + data_[3] * point[3];
    dist = dist*dist;
    dist/=((data_[0]*data_[0]+data_[1]*data_[1]+data_[2]*data_[2])*
     point[3]*point[3]);
      
    return dist;
  }

  inline void HomgPlane3D::SetFromPoints(const HomgPoint3D& p1,
                                         const HomgPoint3D& p2,
                                         const HomgPoint3D& p3) {
    // set up four submatrices as in "Multiple View Geometry", p.47
    Matrix3x3<HOMGPLANE3D_TYPE>  M234, M134, M124, M123;
    for (unsigned int row=0; row<3; row++) {
      M234[row][0] = p1[row+1];
      M234[row][1] = p2[row+1];
      M234[row][2] = p3[row+1];

      M123[row][0] = p1[row];
      M123[row][1] = p2[row];
      M123[row][2] = p3[row];
    }

    M124[0][0] = p1[0];
    M124[0][1] = p2[0];
    M124[0][2] = p3[0];
    M124[1][0] = p1[1];
    M124[1][1] = p2[1];
    M124[1][2] = p3[1];
    M124[2][0] = p1[3];
    M124[2][1] = p2[3];
    M124[2][2] = p3[3];
    
    M134[0][0] = p1[0];
    M134[0][1] = p2[0];
    M134[0][2] = p3[0];
    M134[1][0] = p1[2];
    M134[1][1] = p2[2];
    M134[1][2] = p3[2];
    M134[2][0] = p1[3];
    M134[2][1] = p2[3];
    M134[2][2] = p3[3];
    
    // plane is constructed by determinants of submatrices
    (*this)[0] = M234.GetDeterminant();
    (*this)[1] = -M134.GetDeterminant();
    (*this)[2] = M124.GetDeterminant();
    (*this)[3] = -M123.GetDeterminant();
  }

  inline int 
  HomgPlane3D::GetLineIntersection(const HomgPoint3D& PointOnLine,
                                   const HomgPoint3D& LineDir,
                                   HomgPoint3D& Intersection) const {
#ifdef BIAS_DEBUG
    if (LineDir[3]!=0.0) {
      BIASERR("Incorrect data for line direction (w==0 required for dir)! "
              <<PointOnLine<<" "<<LineDir);
      BIASABORT;
    }
#endif
    // solve equation for lamda:
    // (PointOnLine + lamda*LineDir) * (this) = 0
    // Intersection = PointOnLine + lamda*LineDir

    // assume Linedir[3]==0 (only direction)
    const double dirproduct = LineDir[0]*data_[0]+LineDir[1]*data_[1]+
      LineDir[2]*data_[2];
    const double pointproduct = PointOnLine[0]*data_[0] + 
      PointOnLine[1]*data_[1] + PointOnLine[2]*data_[2] +  
      PointOnLine[3]*data_[3]; 
    // catch degenerate cases
    if (fabs(dirproduct)<HOMGPLANE3D_EPS) {
      if (fabs(pointproduct)<HOMGPLANE3D_EPS) {
        // lamda = 0/0    ->  arbitrary solution
        Intersection = PointOnLine;
        return -1;
      } else {
        // lamda = x/0    ->  line parallel to plane
        Intersection = PointOnLine;
        Intersection[3] = 0.0;
        return 1;
      }
    }
    const double lamda = pointproduct / dirproduct;
    Intersection = PointOnLine - (LineDir * lamda);
    return 0;
  }

  inline HomgPoint3D HomgPlane3D::ProjectPoint(const HomgPoint3D& 
                                               origPoint) const {
    // search for plane in normal direction
    HomgPoint3D LineDir(data_[0], data_[1], data_[2], 0.0);
    HomgPoint3D projection(0,0,0,0);
    GetLineIntersection(origPoint, LineDir, projection);
    return projection;
  }




} //namespace BIAS
#endif // __HOMGPLANE3D_HH__