vpPlane.cpp

/****************************************************************************
 *
 * $Id: vpPlane.cpp 4740 2014-05-26 07:15:44Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Plane geometrical structure.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <visp/vpPlane.h>

#include <cmath>    // std::fabs
#include <limits>   // numeric_limits

vpPlane&
vpPlane::operator =(const vpPlane& p)
{
  A = p.A ;
  B = p.B ;
  C = p.C ;
  D = p.D ;

  return *this ;
}

vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}

vpPlane::vpPlane(const double a,const double b,const double c, const double d)
  : A(a), B(b), C(c), D(d) {}

vpPlane::vpPlane(const vpPlane& P)
  : A(0), B(0), C(0), D(0)
{
  setA(P.getA()) ;
  setB(P.getB()) ;
  setC(P.getC()) ;
  setD(P.getD()) ;
}

vpPlane::vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame)
  : A(0), B(0), C(0), D(0)
{
  //Equation of the plane is given by:
  A = n[0];
  B = n[1];
  C = n[2];

  if (frame == vpPlane::camera_frame)
    D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
  else
    D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());

}

void vpPlane::init(const vpPlane& P)
{
  setA(P.getA()) ;
  setB(P.getB()) ;
  setC(P.getC()) ;
  setD(P.getD()) ;
}

void vpPlane::init(const vpColVector & P, const vpColVector &n)
{
  //Equation of the plane is given by:
  A = n[0];
  B = n[1];
  C = n[2];

  D=-(A*P[0]+B*P[1]+C*P[2]);
}

void
vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
{
  vpColVector a(3);
  vpColVector b(3);
  vpColVector n(3);
  if (frame == vpPlane::camera_frame) {
    //Calculate vector corresponding to PQ
    a[0]=P.get_X()-Q.get_X();
    a[1]=P.get_Y()-Q.get_Y();
    a[2]=P.get_Z()-Q.get_Z();

    //Calculate vector corresponding to PR
    b[0]=P.get_X()-R.get_X();
    b[1]=P.get_Y()-R.get_Y();
    b[2]=P.get_Z()-R.get_Z();
  }
  else {
    //Calculate vector corresponding to PQ
    a[0]=P.get_oX()-Q.get_oX();
    a[1]=P.get_oY()-Q.get_oY();
    a[2]=P.get_oZ()-Q.get_oZ();

    //Calculate vector corresponding to PR
    b[0]=P.get_oX()-R.get_oX();
    b[1]=P.get_oY()-R.get_oY();
    b[2]=P.get_oZ()-R.get_oZ();
  }
  //Calculate normal vector to plane PQ x PR
  n=vpColVector::cross(a,b);

  //Equation of the plane is given by:
  A = n[0];
  B = n[1];
  C = n[2];
  if (frame == vpPlane::camera_frame)
    D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
  else
    D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());

  double norm =  sqrt(A*A+B*B+C*C) ;
  A /= norm ;
  B /= norm ;
  C /= norm ;
  D /= norm ;
}


vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
  : A(0), B(0), C(0), D(0)
{
  init(P, Q, R, frame) ;
}

vpColVector  vpPlane::getNormal() const
{
  vpColVector n(3);
  n[0] = A ;
  n[1] = B ;
  n[2] = C ;

  return n ;
}

void  vpPlane::getNormal(vpColVector &n) const
{
  n.resize(3) ;
  n[0] = A ;
  n[1] = B ;
  n[2] = C ;
}

void
vpPlane::projectionPointOnPlan(const  vpPoint& P, vpPoint& Pproj) const
{
  double x0,y0,z0 ;
  double rho ;

  x0 = P.get_X()/P.get_W() ;
  y0 = P.get_Y()/P.get_W() ;
  z0 = P.get_Z()/P.get_W() ;

  rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;

  Pproj.set_X(x0+A*rho) ;
  Pproj.set_Y(y0+B*rho) ;
  Pproj.set_Z(z0+C*rho) ;
  Pproj.set_W(1) ;
}


double
vpPlane::rayIntersection(const vpPoint &M0,
             const vpPoint &M1,
             vpColVector &H ) const
{

  double k,scal;
  double R[3];

  //  if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
  if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
     || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
     || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
  {
    R[0]= M1.get_X() - M0.get_X();
    R[1]= M1.get_Y() - M0.get_Y();
    R[2]= M1.get_Z() - M0.get_Z();

    scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
    //if (scal != 0)
    if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
      k =  -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
    else
      k = 0;

    H[0] = M0.get_X()+ k*R[0];
    H[1] = M0.get_Y()+ k*R[1];
    H[2] = M0.get_Z()+ k*R[2];
  }
  else
  {
    scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
    //if (scal != 0)
    if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
      k = -getD()/scal;
    else
      k=0;
    H[0] = k*M1.get_X();
    H[1] = k*M1.get_Y();
    H[2] = k*M1.get_Z();
  }

  return k;

}

double vpPlane::getIntersection(const vpColVector &M1,vpColVector &H )const
{

  double k,scal;

  scal = A*M1[0] + B*M1[1] + C*M1[2];
  //if (scal != 0)
  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
    k = -getD()/scal;
  else
    k=0;
  H[0] = k*M1[0];
  H[1] = k*M1[1];
  H[2] = k*M1[2];

  return k;

}

void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo)
{
  // Save current plane parameters
  double Ao = A, Bo = B, Co = C, Do =D ;
  A = cMo[0][0]*Ao + cMo[0][1]*Bo  + cMo[0][2]*Co;
  B = cMo[1][0]*Ao + cMo[1][1]*Bo  + cMo[1][2]*Co;
  C = cMo[2][0]*Ao + cMo[2][1]*Bo  + cMo[2][2]*Co;
  D = Do - (cMo[0][3]*A + cMo[1][3]*B  + cMo[2][3]*C);
}

VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p)
{
  return (os  << "("<<p.getA() << ","<<p.getB()
    << ","<<p.getC()<< ","<<p.getD() <<") ") ;
} ;