/*!
  This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.

  \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
  \param x : The coordinate of the new points along the x axis and given in the user unit system.
  \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
*/
void vpPlot::plot(const unsigned int graphNum,
                  const double x, const vpPoseVector &v_y)
{
  if((graphList+graphNum)->curveNbr == v_y.getRows())
  {
    for(unsigned int i = 0;i < v_y.getRows();++i)
      this->plot(graphNum, i, x, v_y[i]);
  }
  else
    vpTRACE("error in plot vector : not the right dimension");
}

/*!
  Compute the direct geometric model of the camera in terms of pose vector.

  \param q : Articular position for pan and tilt axis.

  \param r : Pose vector corresponding to the transformation between the robot
  reference frame (called fixed) and the camera frame.

*/
void
vpPtu46::computeMGD (const vpColVector & q, vpPoseVector & r)
{
  vpHomogeneousMatrix fMc;

  computeMGD (q, fMc);
  r.buildFrom(fMc.inverse());

  return ;
}

void
vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc)
{
  vpHomogeneousMatrix fMc;

  get_fMc (q, fMc);
  fvc.buildFrom(fMc.inverse());

  return ;
}

/*!
 * Modify the current control position.
 * \param position :
 */
void vpVirtuose::setPosition (vpPoseVector &position)
{
  init();

  float position_[7];
  vpTranslationVector translation;
  vpQuaternionVector quaternion;

  position.extract(translation);
  position.extract(quaternion);

  for (int i=0; i<3; i++)
    position_[i] = translation[i];
  for (int i=0; i<4; i++)
    position_[3+i] = quaternion[i];

  if (virtSetPosition(m_virtContext, position_)) {
    int err = virtGetErrorCode(m_virtContext);
    throw(vpException(vpException::fatalError,
                      "Error calling virtSetPosition: error code %d", err));
  }
}

vpColVector::vpColVector (const vpPoseVector &p)
  : vpArray2D<double>(p.size(), 1)
{
  for (unsigned int i=0; i< p.size(); i++)
    (*this)[i] = p[i];
}