vpHomography::vpHomography(const vpThetaUVector &tu,
                           const vpTranslationVector &atb,
                           const vpPlane &p)
  : vpArray2D<double>(3,3), aMb(), bP()
{
  buildFrom(tu, atb, p) ;
}

vpHomography::vpHomography(const vpThetaUVector &tu,
			   const vpTranslationVector &atb,
			   const vpPlane &_bP) : vpMatrix()
{
  init() ;
  buildFrom(tu,atb,_bP) ;
}

/*!  
Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
representation vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpRzyxVector& rzyx)
{
  vpRotationMatrix R(rzyx) ;

  buildFrom(R) ;
  return *this ;
}

/*!
  Build a 6 dimension pose vector \f$ [\bf t, \Theta \bf u]^\top\f$ from
  an homogeneous matrix \f$ \bf M \f$.

  \param M : Homogeneous matrix \f$ \bf M \f$ from which translation \f$
  \bf t \f$ and \f$\Theta \bf u \f$ vectors are extracted to initialize
  the pose vector.

  \return The build pose vector.

*/
vpPoseVector
vpPoseVector::buildFrom(const vpHomogeneousMatrix& M)
{
  vpRotationMatrix R ;    M.extract(R) ;
  vpTranslationVector t ; M.extract(t) ;
  buildFrom(t,R) ;
  return *this ;
}

/*!

  Constructor that build a 3D visual feature from an homogeneous
  matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.

  \param f2Mf1 [in] : 3D displacement that the camera has to achieve to
  move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
  \param r : type of feature. It can be vpFeature::cdMc or vpFeature::cMo.

*/
vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1, vpFeatureTranslationRepresentationType r) : vpBasicFeature()
{
  init() ;

  translation = r;

  buildFrom(f2Mf1) ;
}

/*!
  Build a 3D \f$ \theta u \f$ visual feature from an
  homogeneous matrix \f$ M \f$ that represent the displacement that
  the camera has to achieve.

  \param M [in] : Homogeneous transformation that describe the
  movement that the camera has to achieve. Only the rotational part of
  this homogeneous transformation is taken into consideration.
  Depending on the rotation representation type
  (vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
  construct this object, the parameter \e M represents either the
  rotation that the camera has to achieve to move from the desired
  camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
  that the camera has to achieve to move from the current camera frame
  to the desired one (\f$ ^{c}R_{c^*}\f$).


*/
void
vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
    vpRotationMatrix R ;
    M.extract(R)  ;
    vpThetaUVector tu(R) ;
    buildFrom(tu) ;
}

vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu, 
                                 vpFeatureThetaURotationRepresentationType r)
  : rotation(r)
{
  init() ;
  
  buildFrom(tu) ;
}

/*!

  Initialize a force/torque twist transformation matrix from a translation vector
  \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.

  \param tx,ty,tz : Translation vector in meters.

  \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz,
                                       const double tux, const double tuy, const double tuz)
  : vpArray2D<double>(6,6)
{
  vpTranslationVector T(tx,ty,tz) ;
  vpThetaUVector tu(tux,tuy,tuz) ;
  buildFrom(T,tu) ;  
}

/*!

  Initialize a force/torque twist transformation matrix from a translation vector
  \e t and a rotation vector with \f$\theta u \f$ parametrization.

  \param tv : Translation vector.
  
  \param thetau : \f$\theta {\bf u}\f$ rotation vector.

*/
vpForceTwistMatrix
vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv,
                              const vpThetaUVector &thetau)
{
  vpRotationMatrix R ;
  R.buildFrom(thetau) ;
  buildFrom(tv,R) ;
  return (*this) ;
}

/*!
  Initialise the triangle thanks to the collection of 2D points (in meter). The
  fields \e x and \e y are used to compute the corresponding coordinates in
  pixel thanks to the camera parameters \e cam.

  \warning the corners must be ordered (either clockwise or counter clockwise).

  \param corners : The corners of the polyon.
  \param cam : The camera parameters used to convert the coordinates from meter
  to pixel.
*/
void
vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
{
  std::vector<vpImagePoint> ipCorners(corners.size());
  for(unsigned int i=0; i<corners.size(); ++i){
    vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
  }
  buildFrom(ipCorners);
}

/*!

  Initialize a velocity twist transformation matrix from a translation vector
  \e t and a rotation vector with \f$\theta u \f$ parametrization.

  \param t : Translation vector.
  
  \param thetau : \f$\theta {\bf u}\f$ rotation vector.

*/
vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
                                 const vpThetaUVector &thetau)
{
  vpRotationMatrix R ;
  R.buildFrom(thetau) ;
  buildFrom(t,R) ;
  return (*this) ;
}

/*! 
  Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$
  Euler angles vector.
  \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
  input.
  \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.   
*/
vpRzyzVector
vpRzyzVector::buildFrom(const vpThetaUVector& tu)
{
  vpRotationMatrix R ;
  R.buildFrom(tu) ;
  buildFrom(R) ;

  return *this ;
}

vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
					 const double ty,
					 const double tz,
					 const double tux,
					 const double tuy,
					 const double tuz) : vpMatrix()
{
  init() ;
  buildFrom(tx, ty, tz,tux, tuy, tuz) ;
}

/*!
Converts an homogeneous matrix into a \f$\theta {\bf u}\f$ vector.
*/
vpThetaUVector
vpThetaUVector::buildFrom(const vpHomogeneousMatrix& M)
{
  vpRotationMatrix R;

  M.extract(R);
  buildFrom(R);

  return *this ;
}

/*!

  Initialize a force/torque twist transformation matrix from an homogeneous matrix
  \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
  \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]

  \param M : Homogeneous matrix \f$M\f$ used to initialize the force/torque twist
  transformation matrix.

*/
vpForceTwistMatrix
vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  vpTranslationVector t ;
  vpRotationMatrix R ;
  M.extract(R) ;
  M.extract(t) ;
  
  buildFrom(t, R) ;
  return (*this) ;
}

/*!

  Initialize a force/torque twist transformation matrix from a translation vector
  \e t and a rotation vector with \f$\theta u \f$ parametrization.

  \param tx,ty,tz : Translation vector in meters.

  \param tux,tuy,tuz : \f$\theta u\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx,
				       const double ty,
				       const double tz,
				       const double tux,
				       const double tuy,
				       const double tuz) : vpMatrix()
{
  init() ;
  vpTranslationVector T(tx,ty,tz) ;
  vpThetaUVector tu(tux,tuy,tuz) ;
  buildFrom(T,tu) ;  
}

/*! 
  Convert a rotation matrix into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
  angles vector.
  
  \param R : Rotation matrix used as input.
  \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.   
*/
vpRxyzVector
vpRxyzVector::buildFrom(const vpRotationMatrix& R)
{
  double COEF_MIN_ROT = 1e-6;
  double phi ;

  if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) phi = 0 ;
  else phi = atan2(-R[1][2], R[2][2]) ;

  double si = sin(phi) ;
  double co = cos(phi) ;
  double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
  double psi = atan2(co*R[1][0] + si*R[2][0], co*R[1][1] + si*R[2][1]);

  buildFrom(phi, theta, psi);

  return *this ;
}

/*! 
  Convert a rotation matrix into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
  angles vector.
  
  \param R : Rotation matrix used as input.
  \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.   
*/
vpRzyzVector
vpRzyzVector::buildFrom(const vpRotationMatrix& R)
{
  double phi ;
  if ((fabs(R[1][2]) < 1e-6) &&(fabs(R[0][2]) < 1e-6))
    phi = 0 ;
  else
    phi = atan2(R[1][2],R[0][2]) ;
  double cphi = cos(phi) ;
  double sphi = sin(phi) ;

  double theta = atan2(cphi*R[0][2]+sphi*R[1][2],R[2][2]);

  double psi = atan2(-sphi*R[0][0]+cphi*R[1][0],-sphi*R[0][1]+cphi*R[1][1]) ;

  buildFrom(phi, theta, psi);

  return *this ;
}

/*!
  Initialise the polygon by (left-)clicking to add a corners to the polygon.
  A right click is used to stop the addition of new corner.

  \param I : The image where to click to initialise the corners.
*/
void
vpPolygon::initClick(const vpImage<unsigned char>& I)
{
  vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
  vpImagePoint ip;

  std::vector<vpImagePoint> cornersClick;

  while(button == vpMouseButton::button1){
    vpDisplay::getClick(I, ip, button, true);
    if(button == vpMouseButton::button1){
      vpDisplay::displayCross(I, ip, 5, vpColor::red);
      cornersClick.push_back(ip);
      vpDisplay::flush(I);
    }
  }

  buildFrom(cornersClick);
}

/*!
  Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
  angles vector from a \f$\theta {\bf u}\f$ vector.
  \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
  input to initialize the Euler angles.
*/
vpRzyzVector::vpRzyzVector(const vpThetaUVector& tu)
  : vpRotationVector (3)
{
  buildFrom(tu) ;
}