OctreeObject.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLGeom/src/ICLGeom/OctreeObject.h                     **
** Module : ICLGeom                                                **
** Authors: Christof Elbrechter                                    **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLUtils/CompatMacros.h>
#include <ICLMath/Octree.h>
#include <ICLGeom/SceneObject.h>
#include <ICLGeom/PointCloudObjectBase.h>
#include <ICLGeom/Camera.h>


#ifdef ICL_SYSTEM_APPLE
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#elif WIN32
#define NOMINMAX
#include <Windows.h>
#include <GL/gl.h>
#include <GL/glu.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#endif

namespace icl{
  namespace geom{

    ICLGeom_API void octree_object_render_box(float x0, float y0, float z0,
                                              float x1, float y1, float z1);

    template<class Scalar, int CAPACITY, int SF, class Pt, int ALLOC_CHUNK_SIZE>
    struct OctreePointRenderer{
      typedef typename math::Octree<Scalar,CAPACITY,SF,Pt,ALLOC_CHUNK_SIZE>::Node Node;
      static void render(const Node *node){
        glBegin(GL_POINTS);
        for(const Pt *p = node->points; p < node->next;++p){
          glVertex3f( (*p)[0],(*p)[1],(*p)[2]);
        }
        glEnd();
      }
    };

#if 0
    template<int CAPACITY, int SF, int ALLOC_CHUNK_SIZE>
    struct OctreePointRenderer<float,CAPACITY,SF,math::FixedColVector<float,4>,ALLOC_CHUNK_SIZE>{
      typedef FixedColVector<float,4> Pt;
      typedef typename math::Octree<float,CAPACITY,SF,Pt,ALLOC_CHUNK_SIZE>::Node Node;
      static void render(const Node *node){
        glEnableClientState(GL_VERTEX_ARRAY);
        glVertexPointer(4,GL_FLOAT,0,node->points);
        glDrawArrays(GL_POINTS, 0, (int)(node->next - node->points));
        glDisableClientState(GL_VERTEX_ARRAY);
      }
    };

    template<int CAPACITY, int SF, int ALLOC_CHUNK_SIZE>
    struct OctreePointRenderer<icl32s,CAPACITY,SF,math::FixedColVector<icl32s,4>,ALLOC_CHUNK_SIZE>{
      typedef FixedColVector<icl32s,4> Pt;
      typedef typename math::Octree<icl32s,CAPACITY,SF,Pt,ALLOC_CHUNK_SIZE>::Node Node;
      static void render(const Node *node){
        glEnableClientState(GL_VERTEX_ARRAY);
        glVertexPointer(4,GL_INT,0,node->points);
        glDrawArrays(GL_POINTS, 0, (int)(node->next - node->points));
        glDisableClientState(GL_VERTEX_ARRAY);
      }
    };
#endif


    template<class Scalar, int CAPACITY=4, int SF=32, class Pt=math::FixedColVector<Scalar,4>, int ALLOC_CHUNK_SIZE=1024>
    class OctreeObject : public math::Octree<Scalar,CAPACITY,SF,Pt,ALLOC_CHUNK_SIZE>, public SceneObject{

      typedef math::Octree<Scalar,CAPACITY,SF,Pt, ALLOC_CHUNK_SIZE> Parent;
      bool m_renderPoints;     
      bool m_renderBoxes;      
      GeomColor m_pointColor;  
      GeomColor m_boxColor;    

      protected:
      void init(){
        m_renderPoints = false;
        m_renderBoxes = true;
        m_pointColor = GeomColor(0,0.5,1,1);
        m_boxColor = GeomColor(0,1,0,0.3);
        setLockingEnabled(true);
      }

      public:

      OctreeObject(const Scalar &minX, const Scalar &minY, const Scalar &minZ,
                   const Scalar &width, const Scalar &height, const Scalar &depth):
      Parent(minX,minY,minZ,width,height,depth){
        init();
        SceneObject::setLockingEnabled(true);
      }

      OctreeObject(const Scalar &min, const Scalar &len):Parent(min,len){
        init();
      }


      void fill(const PointCloudObjectBase &obj, bool clearBefore=true){
        SceneObject::lock();
        if(clearBefore) Parent::clear();
        if(obj.supports(PointCloudObjectBase::XYZH)){
          const core::DataSegment<float,4> xyzh = obj.selectXYZH();
          for(int i=0;i<xyzh.getDim();i+=1){
            Parent::insert(xyzh[i]);
          }
        }else if(obj.supports(PointCloudObjectBase::XYZ)){
          const core::DataSegment<float,3> xyz = obj.selectXYZ();
          for(int i=0;i<xyz.getDim();i+=1){
            Parent::insert(xyz[i].resize<1,4>(1));
          }
        }
        SceneObject::unlock();
      }


      template<class Filter>
      void fill(const PointCloudObjectBase &obj, Filter f, bool clearBefore){
        SceneObject::lock();
        if(clearBefore) Parent::clear();
        if(obj.supports(PointCloudObjectBase::XYZH)){
          const core::DataSegment<float,4> xyzh = obj.selectXYZH();
          for(int i=0;i<xyzh.getDim();i+=1){
            if(f(xyzh[i])){
              Parent::insert(xyzh[i]);
            }
          }
        }else if(obj.supports(PointCloudObjectBase::XYZ)){
          const core::DataSegment<float,3> xyz = obj.selectXYZ();
          for(int i=0;i<xyz.getDim();i+=1){
            if(f(xyz[i])){
              Parent::insert(xyz[i].resize<1,4>(1));
            }
          }
        }
        SceneObject::unlock();
      }



      struct PointFilter {
        math::Vec3 pos;   
        float  sqrRadius; 

        PointFilter(const Vec &p, float radius=10){
          pos = p.resize<1,3>();
          sqrRadius = radius*radius;
        }


        PointFilter(const geom::Camera &cam){
          pos = cam.getPosition().resize<1,3>();
          this->sqrRadius = utils::sqr(cam.getFocalLength());
        }


        template<int N>
        inline bool operator()(const math::FixedColVector<float,N> &v) const{
          return ( ( sqr(v[0]-pos[0]) +
                     sqr(v[1]-pos[1]) +
                     sqr(v[2]-pos[2]) ) > sqrRadius);
        }
      };



      void setRenderPoints(bool enabled) {
        m_renderPoints = enabled;
      }

      bool getRenderPoints() const {
        return m_renderPoints;
      }

      void setRenderBoxes(bool enabled) {
        m_renderBoxes= enabled;
      }

      bool getRenderBoxes() const {
        return m_renderBoxes;
      }

      void setBoxColor(const GeomColor &color){
        m_boxColor = color/255;
      }

      GeomColor getBoxColor() const{
        return m_boxColor*255;
      }


      void setPointColor(const GeomColor &color){
        m_pointColor = color/255;
      }

      GeomColor getPointColor() const{
        return m_pointColor*255;
      }

      virtual void customRender(){
        if(!m_renderPoints && !m_renderBoxes) return;
        glMatrixMode(GL_MODELVIEW);
        glPushMatrix();

        glScalef(1./SF,1./SF,1./SF);

        GLboolean lightWasOn = true;
        glGetBooleanv(GL_LIGHTING,&lightWasOn);
        glDisable(GL_LIGHTING);

        glPointSize(m_pointSize);
        if(m_renderPoints){
          renderNodeWithPoints(Parent::root);
        }else{
          glColor4fv(m_boxColor.data());
          renderNode(Parent::root);
        }
        if(lightWasOn){
          glEnable(GL_LIGHTING);
        }

        glPopMatrix();
      }

      protected:

      void box(const typename Parent::AABB &bb) const {
        const Pt &c = bb.center, s = bb.halfSize;
        octree_object_render_box(c[0] - s[0],c[1] - s[1],c[2] - s[2],
                                 c[0] + s[0],c[1] + s[1],c[2] + s[2]);

      }

      void renderNodeWithPoints(const typename Parent::Node *node) const  {
        if(m_renderBoxes){
          glColor4fv(m_boxColor.data());
          box(node->boundary);
        }

        glColor4fv(m_pointColor.data());
        OctreePointRenderer<Scalar,CAPACITY,SF,Pt,ALLOC_CHUNK_SIZE>::render(node);

        if(node->children){
          for(int i=0;i<8;++i){
            renderNodeWithPoints(node->children+i);
          }
        }
      }

      void renderNode(const typename Parent::Node *node) const{
        box(node->boundary);
        if(node->children){
          for(int i=0;i<8;++i){
            renderNode(node->children+i);
          }
        }
      }
    };

  }

}