vpWireFrameSimulator.h

/****************************************************************************
 *
 * $Id: vpWireFrameSimulator.h 4056 2013-01-05 13:04:42Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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:
 * Wire frame simulator
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/


#ifndef vpWireFrameSimulator_HH
#define vpWireFrameSimulator_HH

#include <visp/vpConfig.h>
#include <stdio.h>
#include <iostream>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits
#include <list>
#include <string>

extern "C" {
#include <visp/vpMy.h>
#include <visp/vpArit.h>
#include <visp/vpBound.h>
#include <visp/vpView.h>
#include <visp/vpToken.h>
#include <visp/vpTmstack.h>
#include <visp/vpVwstack.h>

int open_display();
int close_display();
int open_clipping();
int close_clipping();
int open_keyword (Keyword *kwp);
int open_lex (void);
int open_source (FILE *fd, const char *str);
int malloc_Bound_scene (Bound_scene *bsp, const char *name,Index bn);
int free_Bound_scene (Bound_scene *bsp);
int parser (Bound_scene *bsp);
int close_source (void);
int close_lex (void);
int close_keyword (void);
void add_rfstack (int i);
void load_rfstack (int i);
void add_vwstack (const char* path, ... );
void display_scene(Matrix mat, Bound_scene sc);
int * get_rfstack (void);
Matrix  * get_tmstack (void);
int View_to_Matrix (View_parameters *vp, Matrix m);
void postmult_matrix (Matrix a, Matrix b);
Bound *clipping_Bound (Bound *bp, Matrix m);
int set_Bound_face_display (Bound *bp, Byte b);
int point_3D_2D (Point3f *p3, Index size, unsigned int xsize, unsigned int ysize, Point2i *p2);
void point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4);
int wireframe_Face (Face *fp, Point2i *pp);
}

#include <visp/vpConfig.h>
#include <visp/vpImage.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpDisplay.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
# include <visp/vpList.h>
#endif
#include <visp/vpImagePoint.h>
#include <visp/vpImageSimulator.h>

void set_scene (const char*, Bound_scene *, float);
void vp2jlc_matrix (const vpHomogeneousMatrix, Matrix&);


class VISP_EXPORT vpWireFrameSimulator
{
public:

  typedef enum
  {
    THREE_PTS,
    CUBE,
    PLATE,
    SMALL_PLATE,
    RECTANGLE,
    SQUARE_10CM,
    DIAMOND,
    TRAPEZOID,
    THREE_LINES,
    ROAD,
    TIRE,
    PIPE,
    CIRCLE,
    SPHERE,
    CYLINDER,
    PLAN,
    POINT_CLOUD,
  } vpSceneObject;

  typedef enum
  {
    D_STANDARD,
    D_CIRCLE,
    D_TOOL
  } vpSceneDesiredObject;

  typedef enum
  {
    CT_LINE,
    CT_POINT
  } vpCameraTrajectoryDisplayType;

protected:
  Bound_scene scene;
  Bound_scene desiredScene;
  Bound_scene camera;
  std::list<vpImageSimulator> objectImage;

  vpHomogeneousMatrix fMo;
  vpHomogeneousMatrix fMc;
  vpHomogeneousMatrix camMf;
  vpHomogeneousMatrix refMo;
  vpHomogeneousMatrix cMo;
  vpHomogeneousMatrix cdMo;

  vpSceneObject object;
  vpSceneDesiredObject desiredObject;

  vpColor camColor;
  vpColor camTrajColor;
  vpColor curColor;
  vpColor desColor;

  bool sceneInitialized;

  bool displayCameraTrajectory;
  std::list<vpImagePoint> cameraTrajectory;
  std::list<vpHomogeneousMatrix> poseList;
  std::list<vpHomogeneousMatrix> fMoList;
  unsigned int nbrPtLimit;

  vpImagePoint old_iPr;
  vpImagePoint old_iPz;
  vpImagePoint old_iPt;
  bool blockedr;
  bool blockedz;
  bool blockedt;
  bool blocked;

  vpHomogeneousMatrix camMf2;
  vpHomogeneousMatrix  f2Mf;

  double px_int;
  double py_int;
  double px_ext;
  double py_ext;

  bool displayObject;
  bool displayDesiredObject;
  bool displayCamera;
  bool displayImageSimulator;

  float cameraFactor;

  vpCameraTrajectoryDisplayType camTrajType;

  bool extCamChanged;

  vpHomogeneousMatrix rotz;

private:
  std::string scene_dir;
  
public:
  vpWireFrameSimulator();
  virtual ~vpWireFrameSimulator();

  inline void deleteCameraPositionHistory() {
    cameraTrajectory.clear();
    poseList.clear();
    fMoList.clear();}

  void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
  void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);

  vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const {
    //if(px_ext != 1 && py_ext != 1)
    // we assume px_ext and py_ext > 0
    if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
        && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
    else
    {
      unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
      return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
    }
  }
  vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const {
    //if(px_ext != 1 && py_ext != 1)
    // we assume px_ext and py_ext > 0
    if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
        && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
    else
    {
      unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
      return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
    }
  }
  inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf;}

  void getExternalImage(vpImage<unsigned char> &I);
  void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
  void getExternalImage(vpImage<vpRGBa> &I);
  void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);

  vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const {
    //if(px_int != 1 && py_int != 1)
    // we assume px_int and py_int > 0
    if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
        && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
    else
    {
      unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
      return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
    }
  }
  vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const {
    //if(px_int != 1 && py_int != 1)
    // we assume px_int and py_int > 0
    if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
        && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
    else
    {
      unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
      return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
    }
  }

  void getInternalImage(vpImage<unsigned char> &I);
  void getInternalImage(vpImage<vpRGBa> &I);

  vpHomogeneousMatrix get_cMo() const {return rotz*cMo;}

  void get_cMo_History(std::list<vpHomogeneousMatrix>& cMo_history) {
    cMo_history.clear();
    for(std::list<vpHomogeneousMatrix>::const_iterator it=poseList.begin(); it!=poseList.end(); ++it){
      cMo_history.push_back(rotz*(*it));
    }
  }

  vpHomogeneousMatrix get_fMo() const {return fMo;}

  void get_fMo_History(std::list<vpHomogeneousMatrix>& fMo_history) {fMo_history = fMoList;}

  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
  void initScene(const char* obj, const char* desiredObject);
  void initScene(const vpSceneObject &obj);
  void initScene(const char* obj);

  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject, const std::list<vpImageSimulator> &imObj);
  void initScene(const char* obj, const char* desiredObject, const std::list<vpImageSimulator> &imObj);
  void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
  void initScene(const char* obj, const std::list<vpImageSimulator> &imObj);

  void setCameraColor(const vpColor &col) {camColor = col;}
  void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo) {this->cMo = rotz * cMo; fMc = fMo*this->cMo.inverse();}

  void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc) {this->fMc = fMc*rotz; cMo = this->fMc.inverse()*fMo;}

  inline void setCameraSizeFactor (const float factor) {cameraFactor = factor;}

  void setCameraTrajectoryColor(const vpColor &col) {camTrajColor = col;}

  inline void setCameraTrajectoryDisplayType (const vpCameraTrajectoryDisplayType &camTrajType) {this->camTrajType = camTrajType;}

  void setCurrentViewColor(const vpColor &col) {curColor = col;}
  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo) {this->cdMo = rotz * cdMo;}
  void setDesiredViewColor(const vpColor &col) {desColor = col;}
  void setDisplayCameraTrajectory (const bool &displayCameraTrajectory) {this->displayCameraTrajectory = displayCameraTrajectory;}

  inline void setExternalCameraParameters(const vpCameraParameters &cam) {
    px_ext = cam.get_px();
    py_ext = cam.get_py();
  }
  void setExternalCameraPosition(const vpHomogeneousMatrix &camMf)
  {
    this->camMf = rotz * camMf;
    vpTranslationVector T;
    this->camMf.extract (T);
    this->camMf2.buildFrom(0,0,T[2],0,0,0);
    f2Mf = camMf2.inverse()*this->camMf;
    extCamChanged = true;
  }

  inline void setInternalCameraParameters(const vpCameraParameters &cam) {
    px_int = cam.get_px();
    py_int = cam.get_py();
  }

  inline void setNbPtTrajectory(const unsigned int nbPt) {nbrPtLimit = nbPt;}

  void set_fMo(const vpHomogeneousMatrix &fMo) {this->fMo = fMo;/*this->cMo = fMc.inverse()*fMo;*/}

protected:
  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
  vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
  vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
  vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
  vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
  vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
  vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);

public:

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

  vp_deprecated void displayTrajectory(const vpImage<unsigned char> &I, vpList<vpHomogeneousMatrix> &list_cMo, vpList<vpHomogeneousMatrix> &list_fMo, vpHomogeneousMatrix camMf);
  vp_deprecated void displayTrajectory(const vpImage<vpRGBa> &I, vpList<vpHomogeneousMatrix> &list_cMo, vpList<vpHomogeneousMatrix> &list_fMo, vpHomogeneousMatrix camMf);

  vp_deprecated vpList<vpHomogeneousMatrix> get_cMo_History () {
    vpList<vpHomogeneousMatrix> list_cMo;
    vpHomogeneousMatrix tmp_;
    for(std::list<vpHomogeneousMatrix>::const_iterator it=poseList.begin(); it!=poseList.end(); ++it){
      tmp_ = (rotz*(*it));
      list_cMo.addRight(tmp_);
    }
    return list_cMo;}
  vp_deprecated vpList<vpHomogeneousMatrix> get_fMo_History () {
    vpList<vpHomogeneousMatrix> fMoHistory;
    for(std::list<vpHomogeneousMatrix>::const_iterator iter = fMoList.begin(); iter != fMoList.end(); ++iter){
      fMoHistory.addRight(*iter);
    }
    return fMoHistory;}


  vp_deprecated void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject, vpList<vpImageSimulator> &imObj);
  vp_deprecated void initScene(const char* obj, const char* desiredObject, vpList<vpImageSimulator> &imObj);
  vp_deprecated void initScene(vpSceneObject obj, vpList<vpImageSimulator> &imObj);
  vp_deprecated void initScene(const char* obj, vpList<vpImageSimulator> &imObj);

#endif
};

#endif