vpWireFrameSimulator.h

/****************************************************************************
 *
 * $Id: vpWireFrameSimulator.h 3530 2012-01-03 10:52:12Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 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();
    
    void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject);
    void initScene(const char* obj, const char* desiredObject);
    void initScene(vpSceneObject obj);
    void initScene(const char* obj);

    void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject, const std::list<vpImageSimulator> &imObj);
    void initScene(const char* obj, const char* desiredObject, const std::list<vpImageSimulator> &imObj);
    void initScene(vpSceneObject obj, const std::list<vpImageSimulator> &imObj);
    void initScene(const char* obj, const std::list<vpImageSimulator> &imObj);
    
    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;}
    
    void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo) {this->cdMo = rotz * cdMo;}
    
    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 vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf;}
    
    void setCameraColor(const vpColor col) {camColor = col;}
    
    void setCameraTrajectoryColor(const vpColor col) {camTrajColor = col;}
    
    void setCurrentViewColor(const vpColor col) {curColor = col;}
    
    void setDesiredViewColor(const vpColor col) {desColor = col;}
    
    void setDisplayCameraTrajectory (const bool displayCameraTrajectory) {this->displayCameraTrajectory = displayCameraTrajectory;}
    
    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);
      }
    }
    
    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 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);
      }
    }
    
    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);
      }
    }
    
    void set_fMo(const vpHomogeneousMatrix &fMo) {this->fMo = fMo;this->cMo = fMc.inverse()*fMo;}
    
    vpHomogeneousMatrix get_fMo() const {return fMo;}
    
    vpHomogeneousMatrix get_cMo() const {return rotz*cMo;}
    
    inline void setInternalCameraParameters(const vpCameraParameters cam) {
      px_int = cam.get_px();
      py_int = cam.get_py();
    }
    
    inline void setExternalCameraParameters(const vpCameraParameters cam) {
      px_ext = cam.get_px();
      py_ext = cam.get_py();
    }
    
    inline void setNbPtTrajectory(const unsigned int nbPt) {nbrPtLimit = nbPt;}
    
    inline void setCameraSizeFactor (const float factor) {cameraFactor = factor;}
    
    inline void deleteCameraPositionHistory() {
      cameraTrajectory.clear();
      poseList.clear();
      fMoList.clear();}
      
    inline void setCameraTrajectoryDisplayType (const vpCameraTrajectoryDisplayType camTrajType) {this->camTrajType = camTrajType;}
    
    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));
      }
    }
    
    void get_fMo_History(std::list<vpHomogeneousMatrix>& fMo_history) {fMo_history = fMoList;}
    
    void displayTrajectory(vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
    
    void displayTrajectory(vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
    
    void getInternalImage(vpImage<vpRGBa> &I);
    void getExternalImage(vpImage<vpRGBa> &I);
    void getExternalImage(vpImage<vpRGBa> &I, vpHomogeneousMatrix camMf);
    
    void getInternalImage(vpImage<unsigned char> &I);
    void getExternalImage(vpImage<unsigned char> &I);
    void getExternalImage(vpImage<unsigned char> &I, vpHomogeneousMatrix camMf);
    
  protected:
    void display_scene(Matrix mat, Bound_scene &sc, vpImage<vpRGBa> &I, vpColor color);
    void display_scene(Matrix mat, Bound_scene &sc, vpImage<unsigned char> &I, vpColor color);
    vpHomogeneousMatrix navigation(vpImage<vpRGBa> &I, bool &changed);
    vpHomogeneousMatrix navigation(vpImage<unsigned char> &I, bool &changed);
    vpImagePoint projectCameraTrajectory (vpImage<vpRGBa> &I, vpHomogeneousMatrix cMo, vpHomogeneousMatrix fMo);
    vpImagePoint projectCameraTrajectory (vpImage<unsigned char> &I, vpHomogeneousMatrix cMo, vpHomogeneousMatrix fMo);
    vpImagePoint projectCameraTrajectory (vpImage<vpRGBa> &I, vpHomogeneousMatrix cMo, vpHomogeneousMatrix fMo, vpHomogeneousMatrix cMf);
    vpImagePoint projectCameraTrajectory (vpImage<unsigned char> &I, vpHomogeneousMatrix cMo, vpHomogeneousMatrix fMo, vpHomogeneousMatrix cMf);

  public:

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

    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 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 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);

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

#endif