Visual Servoing Platform  version 3.2.1 under development (2019-10-18) under development (2019-10-18)
vpMbHiddenFaces.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Generic model based tracker. This class declares the methods to implement
33  *in order to have a model based tracker.
34  *
35  * Authors:
36  * Romain Tallonneau
37  * Aurelien Yol
38  *
39  *****************************************************************************/
40 #pragma once
41 
42 #ifndef vpMbHiddenFaces_HH
43 #define vpMbHiddenFaces_HH
44 
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
48 #include <visp3/mbt/vpMbScanLine.h>
49 #include <visp3/mbt/vpMbtPolygon.h>
50 
51 #ifdef VISP_HAVE_OGRE
52 #include <visp3/ar/vpAROgre.h>
53 #endif
54 
55 #include <limits>
56 #include <vector>
57 
58 template <class PolygonType> class vpMbHiddenFaces;
59 
60 template <class PolygonType> void swap(vpMbHiddenFaces<PolygonType> &first, vpMbHiddenFaces<PolygonType> &second);
61 
70 template <class PolygonType = vpMbtPolygon> class vpMbHiddenFaces
71 {
72 private:
74  std::vector<PolygonType *> Lpol;
76  unsigned int nbVisiblePolygon;
77  vpMbScanLine scanlineRender;
78 
79 #ifdef VISP_HAVE_OGRE
80  vpImage<unsigned char> ogreBackground;
81  bool ogreInitialised;
82  unsigned int nbRayAttempts;
83  double ratioVisibleRay;
84  vpAROgre *ogre;
85  std::vector<Ogre::ManualObject *> lOgrePolygons;
86  bool ogreShowConfigDialog;
87 #endif
88 
89  unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears,
90  const double &angleDisappears, bool &changed, bool useOgre = false,
91  bool not_used = false, unsigned int width=0, unsigned int height=0,
92  const vpCameraParameters &cam = vpCameraParameters());
93 
94 public:
96  virtual ~vpMbHiddenFaces();
97  vpMbHiddenFaces(const vpMbHiddenFaces &copy);
99  friend void swap<>(vpMbHiddenFaces &first, vpMbHiddenFaces &second);
100 
101  void addPolygon(PolygonType *p);
102 
103  bool computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
104  bool &changed, bool useOgre, bool not_used, unsigned int width, unsigned int height,
105  const vpCameraParameters &cam, const vpTranslationVector &cameraPos, unsigned int index);
106 
108 
109  void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h);
110 
111  void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector<std::pair<vpPoint, vpPoint> > &lines,
112  const bool &displayResults = false);
113 
114  vpMbScanLine &getMbScanLineRenderer() { return scanlineRender; }
115 
116 #ifdef VISP_HAVE_OGRE
117  void displayOgre(const vpHomogeneousMatrix &cMo);
118 #endif
119 
125  std::vector<PolygonType *> &getPolygon() { return Lpol; }
126 
127 #ifdef VISP_HAVE_OGRE
128  void initOgre(const vpCameraParameters &cam = vpCameraParameters());
129 #endif
130 
136  unsigned int getNbVisiblePolygon() const { return nbVisiblePolygon; }
137 
138 #ifdef VISP_HAVE_OGRE
139 
148  unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; }
149 
155  vpAROgre *getOgreContext() { return ogre; }
156 
166  double getGoodNbRayCastingAttemptsRatio() { return ratioVisibleRay; }
167 #endif
168 
169  bool isAppearing(const unsigned int i) { return Lpol[i]->isAppearing(); }
170 
171 #ifdef VISP_HAVE_OGRE
172 
177  bool isOgreInitialised() { return ogreInitialised; }
178 #endif
179 
187  bool isVisible(const unsigned int i) { return Lpol[i]->isVisible(); }
188 
189 #ifdef VISP_HAVE_OGRE
190  bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
191 #endif
192 
194  inline PolygonType *operator[](const unsigned int i) { return Lpol[i]; }
196  inline const PolygonType *operator[](const unsigned int i) const { return Lpol[i]; }
197 
198  void reset();
199 
200 #ifdef VISP_HAVE_OGRE
201 
211  void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
212  {
213  ogreBackground = vpImage<unsigned char>(h, w, 0);
214  }
215 
225  void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; }
226 
236  void setGoodNbRayCastingAttemptsRatio(const double &ratio)
237  {
238  ratioVisibleRay = ratio;
239  if (ratioVisibleRay > 1.0)
240  ratioVisibleRay = 1.0;
241  if (ratioVisibleRay < 0.0)
242  ratioVisibleRay = 0.0;
243  }
254  inline void setOgreShowConfigDialog(const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }
255 #endif
256 
257  unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
258  const vpHomogeneousMatrix &cMo, const double &angle, bool &changed);
259  unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
260  const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
261  bool &changed);
262  unsigned int setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
263  bool &changed);
264 
265 #ifdef VISP_HAVE_OGRE
266  unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam,
267  const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
268  bool &changed);
269  unsigned int setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
270  bool &changed);
271 #endif
272 
277  inline unsigned int size() const { return (unsigned int)Lpol.size(); }
278 };
279 
283 template <class PolygonType>
284 vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces() : Lpol(), nbVisiblePolygon(0), scanlineRender()
285 {
286 #ifdef VISP_HAVE_OGRE
287  ogreInitialised = false;
288  nbRayAttempts = 1;
289  ratioVisibleRay = 1.0;
290  ogreShowConfigDialog = false;
291  ogre = new vpAROgre();
292  ogreBackground = vpImage<unsigned char>(480, 640, 0);
293 #endif
294 }
295 
300 {
301  for (unsigned int i = 0; i < Lpol.size(); i++) {
302  if (Lpol[i] != NULL) {
303  delete Lpol[i];
304  }
305  Lpol[i] = NULL;
306  }
307  Lpol.resize(0);
308 
309 #ifdef VISP_HAVE_OGRE
310  if (ogre != NULL) {
311  delete ogre;
312  ogre = NULL;
313  }
314 
315  // This is already done by calling "delete ogre"
316  // for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
317  // if (lOgrePolygons[i]!=NULL){
318  // delete lOgrePolygons[i];
319  // }
320  // lOgrePolygons[i] = NULL;
321  // }
322 
323  lOgrePolygons.resize(0);
324 #endif
325 }
326 
330 template <class PolygonType>
332  : Lpol(), nbVisiblePolygon(copy.nbVisiblePolygon), scanlineRender(copy.scanlineRender)
333 #ifdef VISP_HAVE_OGRE
334  ,
335  ogreBackground(copy.ogreBackground), ogreInitialised(copy.ogreInitialised), nbRayAttempts(copy.nbRayAttempts),
336  ratioVisibleRay(copy.ratioVisibleRay), ogre(NULL), lOgrePolygons(), ogreShowConfigDialog(copy.ogreShowConfigDialog)
337 #endif
338 {
339  // Copy the list of polygons
340  for (unsigned int i = 0; i < copy.Lpol.size(); i++) {
341  PolygonType *poly = new PolygonType(*copy.Lpol[i]);
342  Lpol.push_back(poly);
343  }
344 }
345 
346 template <class PolygonType> void swap(vpMbHiddenFaces<PolygonType> &first, vpMbHiddenFaces<PolygonType> &second)
347 {
348  using std::swap;
349  swap(first.Lpol, second.Lpol);
350  swap(first.nbVisiblePolygon, second.nbVisiblePolygon);
351  swap(first.scanlineRender, second.scanlineRender);
352 #ifdef VISP_HAVE_OGRE
353  swap(first.ogreInitialised, second.ogreInitialised);
354  swap(first.nbRayAttempts, second.nbRayAttempts);
355  swap(first.ratioVisibleRay, second.ratioVisibleRay);
356  swap(first.ogreShowConfigDialog, second.ogreShowConfigDialog);
357  swap(first.ogre, second.ogre);
358  swap(first.ogreBackground, second.ogreBackground);
359 #endif
360 }
361 
365 template <class PolygonType>
367 {
368  swap(*this, other);
369 
370  return *this;
371 }
372 
378 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
379 {
380  PolygonType *p_new = new PolygonType;
381  p_new->index = p->index;
382  p_new->setNbPoint(p->nbpt);
383  p_new->isvisible = p->isvisible;
384  p_new->useLod = p->useLod;
385  p_new->minLineLengthThresh = p->minLineLengthThresh;
386  p_new->minPolygonAreaThresh = p->minPolygonAreaThresh;
387  p_new->setName(p->name);
388  p_new->hasOrientation = p->hasOrientation;
389 
390  for (unsigned int i = 0; i < p->nbpt; i++)
391  p_new->p[i] = p->p[i];
392  Lpol.push_back(p_new);
393 }
394 
398 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::reset()
399 {
400  nbVisiblePolygon = 0;
401  for (unsigned int i = 0; i < Lpol.size(); i++) {
402  if (Lpol[i] != NULL) {
403  delete Lpol[i];
404  }
405  Lpol[i] = NULL;
406  }
407  Lpol.resize(0);
408 
409 #ifdef VISP_HAVE_OGRE
410  if (ogre != NULL) {
411  delete ogre;
412  ogre = NULL;
413  }
414 
415  // This is already done by calling "delete ogre"
416  // for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
417  // if (lOgrePolygons[i]!=NULL){
418  // delete lOgrePolygons[i];
419  // }
420  // lOgrePolygons[i] = NULL;
421  // }
422 
423  lOgrePolygons.resize(0);
424 
425  ogreInitialised = false;
426  nbRayAttempts = 1;
427  ratioVisibleRay = 1.0;
428  ogre = new vpAROgre();
429  ogreBackground = vpImage<unsigned char>(480, 640);
430 #endif
431 }
432 
440 template <class PolygonType>
442 {
443  for (unsigned int i = 0; i < Lpol.size(); i++) {
444  // For fast result we could just clip visible polygons.
445  // However clipping all of them gives us the possibility to return more
446  // information in the scanline visibility results
447  // if(Lpol[i]->isVisible())
448  {
449  Lpol[i]->changeFrame(cMo);
450  Lpol[i]->computePolygonClipped(cam);
451  }
452  }
453 }
454 
463 template <class PolygonType>
465  const unsigned int &h)
466 {
467  std::vector<std::vector<std::pair<vpPoint, unsigned int> > > polyClipped(Lpol.size());
468  std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> listPolyClipped;
469  std::vector<int> listPolyIndices;
470 
471  for (unsigned int i = 0; i < Lpol.size(); i++) {
472  // For fast result we could just use visible polygons.
473  // However using all of them gives us the possibility to return more
474  // information in the scanline visibility results
475  // if(Lpol[i]->isVisible())
476  {
477  polyClipped[i].clear();
478  Lpol[i]->getPolygonClipped(polyClipped[i]);
479  if (polyClipped[i].size() != 0) {
480  listPolyClipped.push_back(&polyClipped[i]);
481  listPolyIndices.push_back(Lpol[i]->getIndex());
482  }
483  }
484  }
485 
486  scanlineRender.drawScene(listPolyClipped, listPolyIndices, cam, w, h);
487 }
488 
500 template <class PolygonType>
502  std::vector<std::pair<vpPoint, vpPoint> > &lines,
503  const bool &displayResults)
504 {
505  scanlineRender.queryLineVisibility(a, b, lines, displayResults);
506 }
507 
522 template <class PolygonType>
523 unsigned int vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears,
524  const double &angleDisappears, bool &changed, bool useOgre,
525  bool not_used, unsigned int width, unsigned int height,
526  const vpCameraParameters &cam)
527 {
528  nbVisiblePolygon = 0;
529  changed = false;
530 
531  vpTranslationVector cameraPos;
532 
533  if (useOgre) {
534 #ifdef VISP_HAVE_OGRE
535  cMo.inverse().extract(cameraPos);
536  ogre->renderOneFrame(ogreBackground, cMo);
537 #else
538  vpTRACE("ViSP doesn't have Ogre3D, simple visibility test used");
539 #endif
540  }
541 
542  for (unsigned int i = 0; i < Lpol.size(); i++) {
543  // std::cout << "Calling poly: " << i << std::endl;
544  if (computeVisibility(cMo, angleAppears, angleDisappears, changed, useOgre, not_used, width, height, cam, cameraPos, i))
545  nbVisiblePolygon++;
546  }
547  return nbVisiblePolygon;
548 }
549 
568 template <class PolygonType>
569 bool vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears,
570  const double &angleDisappears, bool &changed, bool useOgre,
571  bool not_used, unsigned int width, unsigned int height,
572  const vpCameraParameters &cam,
573  const vpTranslationVector &cameraPos, unsigned int index)
574 {
575  (void)not_used;
576  unsigned int i = index;
577  Lpol[i]->changeFrame(cMo);
578  Lpol[i]->isappearing = false;
579 
580  // Commented because we need to compute visibility
581  // even when dealing with line in level of detail case
582  /*if(Lpol[i]->getNbPoint() <= 2)
583  {
584  Lpol[i]->isvisible = true;
585  }
586  else*/ {
587  if (Lpol[i]->isVisible()) {
588  bool testDisappear = false;
589  // unsigned int nbCornerInsidePrev = 0;
590 
591  if (!testDisappear) {
592  if (useOgre)
593 #ifdef VISP_HAVE_OGRE
594  testDisappear = ((!Lpol[i]->isVisible(cMo, angleDisappears, true, cam, width, height)) || !isVisibleOgre(cameraPos, i));
595 #else
596  {
597  (void)cameraPos; // Avoid warning
598  testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, width, height));
599  }
600 #endif
601  else
602  testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, width, height));
603  }
604 
605  // test if the face is still visible
606  if (testDisappear) {
607  // std::cout << "Face " << i << " disappears" <<
608  // std::endl;
609  changed = true;
610  Lpol[i]->isvisible = false;
611  } else {
612  // nbVisiblePolygon++;
613  Lpol[i]->isvisible = true;
614 
615  // if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
616  // changed = true;
617  }
618  } else {
619  bool testAppear = true;
620 
621  if (testAppear) {
622  if (useOgre)
623 #ifdef VISP_HAVE_OGRE
624  testAppear = ((Lpol[i]->isVisible(cMo, angleAppears, true, cam, width, height)) && isVisibleOgre(cameraPos, i));
625 #else
626  testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, width, height));
627 #endif
628  else
629  testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, width, height));
630  }
631 
632  if (testAppear) {
633  // std::cout << "Face " << i << " appears" << std::endl;
634  Lpol[i]->isvisible = true;
635  changed = true;
636  // nbVisiblePolygon++;
637  } else {
638  // std::cout << "Problem" << std::endl;
639  Lpol[i]->isvisible = false;
640  }
641  }
642  }
643  // std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon <<
644  // std::endl;
645  return Lpol[i]->isvisible;
646 }
647 
661 template <class PolygonType>
662 unsigned int vpMbHiddenFaces<PolygonType>::setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
663  const vpHomogeneousMatrix &cMo, const double &angle,
664  bool &changed)
665 {
666  return setVisible(width, height, cam, cMo, angle, angle, changed);
667 }
668 
683 template <class PolygonType>
684 unsigned int vpMbHiddenFaces<PolygonType>::setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam,
685  const vpHomogeneousMatrix &cMo, const double &angleAppears,
686  const double &angleDisappears, bool &changed)
687 {
688  return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false, true, width, height, cam);
689 }
690 
702 template <class PolygonType>
703 unsigned int vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears,
704  const double &angleDisappears, bool &changed)
705 {
706  return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false);
707 }
708 
709 #ifdef VISP_HAVE_OGRE
710 
715 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
716 {
717  ogreInitialised = true;
718  ogre->setCameraParameters(cam);
719  ogre->setShowConfigDialog(ogreShowConfigDialog);
720  ogre->init(ogreBackground, false, true);
721 
722  for (unsigned int n = 0; n < Lpol.size(); n++) {
723  Ogre::ManualObject *manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));
724 
725  manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
726  for (unsigned int i = 0; i < Lpol[n]->nbpt; i++) {
727  manual->position((Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(),
728  (Ogre::Real)Lpol[n]->p[i].get_oZ());
729  manual->colour(1.0, 1.0, 1.0);
730  manual->index(i);
731  }
732 
733  manual->index(0);
734  manual->end();
735 
736  ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual);
737 
738  lOgrePolygons.push_back(manual);
739  }
740 }
741 
747 template <class PolygonType> void vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
748 {
749  if (ogreInitialised && !ogre->isWindowHidden()) {
750  for (unsigned int i = 0; i < Lpol.size(); i++) {
751  if (Lpol[i]->isVisible()) {
752  lOgrePolygons[i]->setVisible(true);
753  } else
754  lOgrePolygons[i]->setVisible(false);
755  }
756  ogre->display(ogreBackground, cMo);
757  }
758 }
759 
774 template <class PolygonType>
775 unsigned int vpMbHiddenFaces<PolygonType>::setVisibleOgre(unsigned int width, unsigned int height,
776  const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo,
777  const double &angleAppears, const double &angleDisappears,
778  bool &changed)
779 {
780  return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true, true, width, height, cam);
781 }
782 
794 template <class PolygonType>
795 unsigned int vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears,
796  const double &angleDisappears, bool &changed)
797 {
798  return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true);
799 }
800 
809 template <class PolygonType>
810 bool vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
811 {
812  Ogre::Vector3 camera((Ogre::Real)cameraPos[0], (Ogre::Real)cameraPos[1], (Ogre::Real)cameraPos[2]);
813  if (!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())) {
814  lOgrePolygons[index]->setVisible(false);
815  Lpol[index]->isvisible = false;
816  return false;
817  }
818 
819  // Get the center of gravity
820  bool visible = false;
821  unsigned int nbVisible = 0;
822 
823  for (unsigned int i = 0; i < nbRayAttempts; i++) {
824  Ogre::Vector3 origin(0, 0, 0);
825  Ogre::Real totalFactor = 0.0f;
826 
827  for (unsigned int j = 0; j < Lpol[index]->getNbPoint(); j++) {
828  Ogre::Real factor = 1.0f;
829 
830  if (nbRayAttempts > 1) {
831  int r = rand() % 101;
832 
833  if (r != 0)
834  factor = ((Ogre::Real)r) / 100.0f;
835  }
836 
837  Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(),
838  (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
839  tmp *= factor;
840  origin += tmp;
841  totalFactor += factor;
842  }
843 
844  origin /= totalFactor;
845 
846  Ogre::Vector3 direction = origin - camera;
847  Ogre::Real distanceCollision = direction.length();
848 
849  direction.normalise();
850  Ogre::RaySceneQuery *mRaySceneQuery = ogre->getSceneManager()->createRayQuery(Ogre::Ray(camera, direction));
851  mRaySceneQuery->setSortByDistance(true);
852 
853  Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute();
854  Ogre::RaySceneQueryResult::iterator it = result.begin();
855 
856  // while(it != result.end()){
857  // std::cout << it->movable->getName() << "(" << it->distance<< ") :
858  // " << std::flush; it++;
859  // }
860  // std::cout << std::endl;
861  // it = result.begin();
862 
863  if (it != result.end())
864  if (it->movable->getName().find("SimpleRenderable") !=
865  Ogre::String::npos) // Test if the ogreBackground is intersect in
866  // first
867  ++it;
868 
869  double distance;
870  // In a case of a two-axis aligned segment, ray collision is not always
871  // working.
872  if (Lpol[index]->getNbPoint() == 2 &&
873  (((std::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) <
874  std::numeric_limits<double>::epsilon()) +
875  (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) <
876  std::numeric_limits<double>::epsilon()) +
877  (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) <
878  std::numeric_limits<double>::epsilon())) >= 2)) {
879  if (it != result.end()) {
880  if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
881  nbVisible++;
882  } else {
883  distance = it->distance;
884  // Cannot use epsilon for comparison as ray lenght is slightly
885  // different from the collision distance returned by
886  // Ogre::RaySceneQueryResult.
887  if (distance > distanceCollision || std::fabs(distance - distanceCollision) <
888  1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/)
889  nbVisible++;
890  }
891  } else
892  nbVisible++; // Collision not detected but present.
893  } else {
894  if (it != result.end()) {
895  distance = it->distance;
896  double distancePrev = distance;
897 
898  // std::cout << "For " << Ogre::StringConverter::toString(index) << ":
899  // " << it->movable->getName() << " / " << std::flush;
900 
901  if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
902  nbVisible++;
903  } else {
904  ++it;
905  while (it != result.end()) {
906  distance = it->distance;
907 
908  if (std::fabs(distance - distancePrev) <
909  1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/) {
910  // std::cout << it->movable->getName() << " / " << std::flush;
911  if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
912  nbVisible++;
913  break;
914  }
915  ++it;
916  distancePrev = distance;
917  } else
918  break;
919  }
920  }
921  }
922  }
923 
924  ogre->getSceneManager()->destroyQuery(mRaySceneQuery);
925  }
926 
927  if (((double)nbVisible) / ((double)nbRayAttempts) > ratioVisibleRay ||
928  std::fabs(((double)nbVisible) / ((double)nbRayAttempts) - ratioVisibleRay) <
929  ratioVisibleRay * std::numeric_limits<double>::epsilon())
930  visible = true;
931  else
932  visible = false;
933 
934  if (visible) {
935  lOgrePolygons[index]->setVisible(true);
936  Lpol[index]->isvisible = true;
937  } else {
938  lOgrePolygons[index]->setVisible(false);
939  Lpol[index]->isvisible = false;
940  }
941 
942  return Lpol[index]->isvisible;
943 }
944 #endif // VISP_HAVE_OGRE
945 
946 #endif // vpMbHiddenFaces
vpMbHiddenFaces & operator=(vpMbHiddenFaces other)
unsigned int size() const
bool isAppearing(const unsigned int i)
virtual ~vpMbHiddenFaces()
Implementation of the polygons management for the model-based trackers.
Ogre::Camera * getCamera()
Definition: vpAROgre.h:143
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setShowConfigDialog(const bool showConfigDialog)
Definition: vpAROgre.h:258
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
PolygonType * operator[](const unsigned int i)
operator[] as modifier.
void setOgreShowConfigDialog(const bool showConfigDialog)
vpMbScanLine & getMbScanLineRenderer()
vpHomogeneousMatrix inverse() const
const PolygonType * operator[](const unsigned int i) const
operator[] as reader.
Implementation of an augmented reality viewer using Ogre3D 3rd party.
Definition: vpAROgre.h:90
bool renderOneFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMw)
Definition: vpAROgre.cpp:589
void extract(vpRotationMatrix &R) const
Ogre::SceneManager * getSceneManager()
Definition: vpAROgre.h:163
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMw)
Definition: vpAROgre.cpp:623
void setGoodNbRayCastingAttemptsRatio(const double &ratio)
Class that defines what is a point.
Definition: vpPoint.h:58
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
bool computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed, bool useOgre, bool not_used, unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpTranslationVector &cameraPos, unsigned int index)
bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
vpAROgre * getOgreContext()
unsigned int getNbVisiblePolygon() const
virtual void init(vpImage< unsigned char > &I, bool bufferedKeys=false, bool hidden=false)
Definition: vpAROgre.cpp:115
double getGoodNbRayCastingAttemptsRatio()
friend void swap(vpMbHiddenFaces &first, vpMbHiddenFaces &second)
#define vpTRACE
Definition: vpDebug.h:416
void setCameraParameters(const vpCameraParameters &cameraP)
Definition: vpAROgre.cpp:657
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
Generic class defining intrinsic camera parameters.
std::vector< PolygonType * > & getPolygon()
void addPolygon(PolygonType *p)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
bool isWindowHidden()
Definition: vpAROgre.h:173
bool isVisible(const unsigned int i)
unsigned int getNbRayCastingAttemptsForVisibility()
void displayOgre(const vpHomogeneousMatrix &cMo)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
Class that consider the case of a translation vector.
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)