OpenSceneGraphHelper.cpp

#include "OpenSceneGraphHelper.hh"

#include <Utils/TriangleMesh.hh>


#include <Base/Image/Image.hh>
#include <Filter/Rescale.hh>
#include <Gui/biasgl.h>

#include <Base/Image/ImageIO.hh>

#include <osg/Group>
#include <osg/Geode>
#include <osg/Geometry>
#include <osg/Texture2D>
#include <osg/TexEnv>
#include <osg/MatrixTransform>
#include <osg/Billboard>
#include <osg/ShapeDrawable>
#include <osg/Point>
#include <osg/LineWidth>
#include <osg/LineStipple>

using namespace std;
using namespace BIAS;
using namespace BIAS;

//unsigned char *gid(Image<unsigned char> &img){return img


bool OpenSceneGraphHelper::
BIASimageToOSGimage(const BIAS::Image<unsigned char>& in,osg::Image* out,
                     bool copydata)
{
  unsigned int width  = in.GetWidth();
  unsigned int height = in.GetHeight();
  unsigned int chNum  = in.GetChannelCount();

  BIASASSERT(chNum>0);  
  if(chNum<=4)
  { 
    GLenum colormode = GL_RGBA;
    unsigned int colOff = 4;
    switch(chNum)
    {     
      case 1:  //greyscale image
        //GL_LUMINANCE: RGBA = (X, X, X, 1)  
        //# GL_ALPHA: RGBA = (0, 0, 0, X)
        //# GL_INTENSITY: RGBA = (X, X, X, X)      
        colormode = GL_LUMINANCE;
        colOff = 1;
        break;
      case 2:  //???
        colormode = GL_RGB;
        colOff = 3;      
        break;
      case 3: //rgb
        colormode = GL_RGB;
        colOff = 3;
        break;
      case 4:
        colormode = GL_RGBA;
        colOff = 4;
        break;
      default:        
        BIASERR("unexpected channel count! " << chNum);
        BIASABORT;
    }
    out->allocateImage(width,height,colOff, colormode, GL_UNSIGNED_BYTE);

    /*the texture pixel data is not needed if the bias image is
      exported to disc thus slow copying can be avoided.
    */
    if(copydata)
    {
      void * dest = (void*) out->data();
      BIASASSERT(dest!=NULL);
      void* src =   (void*) in.GetImageData();
      BIASASSERT(src!=NULL);
      if(chNum!=2)
      {
        //copy pixel arrays          
        memcpy( dest, src, width*height*colOff  );       
      }
      else
      {        
        //convert image to rgb ... this might take while....
        for(unsigned int p=0; p<width*height; p++)
        {
          //store data in R and G channels...
          ((unsigned char*) src)[p*3] = ((unsigned char*) dest)[p*2];
          ((unsigned char*) src)[p*3+1] = ((unsigned char*) dest)[p*2+1];
          ((unsigned char*) src)[p*3+2] = 0;
        }
      }
      out->flipVertical() ;
    }
  }
  else
  {
    BIASWARN("texture has too many channels! (channelcount>4)");
    return(false);
  }
  return(true);
}


bool OpenSceneGraphHelper::
OSGimageToBIASimage(const osg::Image* in,
                     BIAS::Image<unsigned char>& out)
{
  if(!in) return false;
  if(in->getDataType() !=GL_UNSIGNED_BYTE)
  {
    BIASWARN("only unsigned byte based textures supported!");
    return false;
  }
  unsigned int width  = in->s();
  unsigned int height = in->t();
  unsigned int chNum  = 3;
  switch(in->getPixelFormat())
  {
    case GL_RGB:
      chNum = 3;
      break;
    case GL_RGBA:
      chNum = 4;
      break;
    case GL_LUMINANCE:
      chNum = 1;
      break;
    default:
      BIASWARN("unsupported pixel format!");
      return false;
  }
  out.Init(width,height,chNum);
  void* dest;
#if defined WIN32 && defined BIAS_BUILD_SHARED_LIBS
  // this is a work-around for for W32, SHARED, DEBUG. 
    const Image<unsigned char>* cip = &out;
    dest = (void*) cip->GetImageData();
#else
  dest = (void*) out.GetImageData();
#endif
  BIASASSERT(dest!=NULL);
  void* src  = (void*) in->data();
  BIASASSERT(src!=NULL);
  memcpy( dest, src, width*height*chNum  );
  out.Flip();
  return true;
}




osg::Node * OpenSceneGraphHelper::
CreateBranchIndexedFaceSets(const TriangleMesh& mesh,
                const std::string& name) 
{
  osg::MatrixTransform* mTrans = new osg::MatrixTransform;
  mTrans->setName(name);
  osg::Geode* mGeode = new osg::Geode();
  osg::Geometry* mGeometry = new osg::Geometry();
  mGeode->addDrawable(mGeometry); 
  mTrans->addChild(mGeode);
 
  const vector<Vector3<double> > &vertexSet = mesh.GetVerticesRef();
  const vector<Vector2<float> > &texCoordSet = mesh.GetTexCoordsRef();
  const vector<Vector<int> > &faceIndices =mesh.GetTriangleIndicesRef();

  //copy vertices:    
  //create new vertex array
  unsigned int vertNumb = vertexSet.size();
  osg::Vec3Array* vertices = new osg::Vec3Array(vertNumb);
  //loop through vertices    
  for(unsigned int vt = 0; vt < vertNumb; vt++)
    (*vertices)[vt].set(vertexSet[vt][0],vertexSet[vt][1],vertexSet[vt][2]);
  //attach vertex array to
  mGeometry->setVertexArray(vertices); 
            
  //set texture coordinates
  unsigned int texCoordNumb = texCoordSet.size();    
  if(texCoordNumb!= vertNumb)
    BIASWARN("Number of texture coordinates does not match vertex number.");
    
  osg::Vec2Array* texCoords = new osg::Vec2Array(texCoordNumb);
  for(unsigned int tc=0; tc<texCoordNumb; tc++)
    (*texCoords)[tc].set(texCoordSet[tc][0],texCoordSet[tc][1]);
  mGeometry->setTexCoordArray(0,texCoords); 
    
  //set faces:
  //loop through faces    
  unsigned int faceNumb = faceIndices.size();
    
  osg::DrawElementsUInt* drawTriangleElmtsU=NULL;
  osg::DrawElementsUInt* drawQuadsElmtsU=NULL;

  for(unsigned int fc = 0; fc<faceNumb ; fc++)
    {
      // get number of indices of actual face
      // this number might vary from face to face although most likely triangles
      // and quads will occure. 
      unsigned int indexNum = faceIndices[fc].Size();
      osg::DrawElementsUInt*  actualFace;
      switch(indexNum)
    {
        case 3:
      //use for all triangles the same "index buffer"
          if(drawTriangleElmtsU==NULL)
        {
          drawTriangleElmtsU = 
        new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
          mGeometry->addPrimitiveSet(drawTriangleElmtsU);            
        }          
          actualFace = drawTriangleElmtsU;
          break;
        case 4:
          if(drawQuadsElmtsU==NULL)
        {
          drawQuadsElmtsU =
        new osg::DrawElementsUInt(osg::PrimitiveSet::QUADS);
          mGeometry->addPrimitiveSet(drawQuadsElmtsU);
        }
          actualFace = drawQuadsElmtsU;
          break;
        default:
      actualFace = 
        new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON);
      mGeometry->addPrimitiveSet(actualFace);
    }
      //loop through indices
      for(unsigned int ix = 0; ix<indexNum; ix++)
    {
      //bias stores face indices as int
      int idx = (faceIndices[fc])[ix];
      BIASASSERT(idx>=0); 
      actualFace->push_back((unsigned int) idx);
    }
    }    
  //is there no color buffer for indexed faces? too bad! :(
  //set and save texture 
  osg::Image*  mImage = new osg::Image;
  
  const bool CopyImage = true;
  if(BIASimageToOSGimage(mesh.GetTextureRef(),mImage,CopyImage)) {
      osg::ref_ptr< osg::Texture2D > mTex2D = new osg::Texture2D;
      mTex2D->setImage( mImage );
                
      osg::StateSet *ss = new osg::StateSet;
      ss->setTextureAttributeAndModes(0, mTex2D.get());
      
      //disable modulation of texture and vertex colors
      //because the bias meshes have no vertex color data.
      osg::TexEnv *txenv = new osg::TexEnv(osg::TexEnv::REPLACE);
      ss->setTextureAttribute(0, txenv);
      
      //delete image after creating opengl texture - is this needed in our case?
      //mTex2D->setUnRefImageDataAfterApply(true);
      mGeode->setStateSet(ss);
    } //biasimage export
 

  return mTrans;
}

bool OpenSceneGraphHelper::OSG2BIASMatrix( const osg::Matrixd& OsgMat,
                                           BIAS::Matrix4x4<double>& BiasMat)
{
  const double* pOSDMatD = OsgMat.ptr();
  double* pBIASMatD = BiasMat.GetData();
  if(pOSDMatD&&pBIASMatD)
  {
    for(int i=0; i<16; i++)
    {
      pBIASMatD[i]=pOSDMatD[i];
    }
    return true;
  }  
  return false;
}

bool OpenSceneGraphHelper::BIAS2OSGMatrix(
                   const BIAS::Matrix4x4<double>& BiasMat,
                   osg::Matrixd& OsgMat)
{
  double* pOSDMatD = OsgMat.ptr();
  const double* pBIASMatD = BiasMat.GetData();
  if(pOSDMatD&&pBIASMatD)
  {
    for(int i=0; i<16; i++)
    {
      pOSDMatD[i]=pBIASMatD[i];
    }
    return true;
  }  
  return false;
}

bool OpenSceneGraphHelper::AdjustImageSizeToGPULimit(BIAS::Image<unsigned char>& tex)
{ 
  BIAS::Image<unsigned char> tp;
  BIAS::Image<unsigned char>* tmpi[2];
  unsigned char current=0;
  unsigned char next=1;
  tmpi[current] = &tex;
  tmpi[next] = &tp;
  //int max_texture_size = 0;

  GLenum format = GL_RGB;
  GLenum internalFormat = GL_RGB;
  if(tex.GetChannelCount()==4)
  {
    format = GL_RGBA;
    internalFormat = GL_RGBA;
  }
  if((tex.GetChannelCount()<3)||(tex.GetChannelCount()>4))
  {
    BIASWARN("AdjustImageSizeToGPULimit not implemented for non rgb(a) images");
    return false;
  }
 
  /*
  If target is GL_PROXY_TEXTURE_2D, no data is read from
    pixels, but all of the texture image state is   recalculated,
    checked for consistency, and checked against the
    implementation's capabilities. If the   implementation cannot
    handle a texture of the requested texture size, it sets all
    of the image state to   0, but does not generate an error (see
    glGetError).
  */
  GLint reswidth = 0; 
  glTexImage2D(GL_PROXY_TEXTURE_2D, 0, internalFormat,  tex.GetWidth(), tex.GetHeight(), 0, format, GL_UNSIGNED_BYTE, NULL);    
  glGetTexLevelParameteriv(GL_PROXY_TEXTURE_2D, 0, GL_TEXTURE_WIDTH, &reswidth); 
  Rescale<unsigned char, unsigned char> rescale;
  while (reswidth==0)
  {
    cout<<"rescaling..."<<endl;
      if (rescale.DownsampleBy2(*(tmpi[current]),*(tmpi[next]))!=0){
          BIASERR("error downsampling");
          return false;
         }
       /// swap current and next indeces
      unsigned char ttt=next;
      next = current;
      current = ttt;
      glTexImage2D(GL_PROXY_TEXTURE_2D, 0, internalFormat,  tmpi[next]->GetWidth(), tmpi[next]->GetHeight(), 0, format, GL_UNSIGNED_BYTE, NULL);          
      glGetTexLevelParameteriv(GL_PROXY_TEXTURE_2D, 0, GL_TEXTURE_WIDTH, &reswidth); 
  }
  //glGetIntegerv(GL_MAX_TEXTURE_SIZE,&max_texture_size);  
  //cout<<"max_texture_size "<<max_texture_size<<endl;
  //if (tmpi[current]->GetWidth()>(unsigned)max_texture_size
  //    || tmpi[current]->GetHeight()>(unsigned)max_texture_size){
  //  // BIASERR("image too big, downsampling");
  //  // downsample
  //  Rescale<unsigned char, unsigned char> rescale;
  //  do {
  //    if (rescale.DownsampleBy2(*(tmpi[current]),*(tmpi[next]))!=0){
  //      BIASERR("error downsampling");
  //    }
  //    // swap current and next indeces
  //    unsigned char ttt=next;
  //    next = current;
  //    current = ttt;
  //  } while ((tmpi[current])->GetWidth()>(unsigned)max_texture_size ||
  //           (tmpi[current])->GetHeight()>(unsigned)max_texture_size);
  //}
  if(current==1) tex = *(tmpi[1]);
  return true;
}

#define INFO_SEP '='

bool OpenSceneGraphHelper::
HasInfo(const osg::ref_ptr<osg::Node> node, const string &key, string &value)
{
  if (!node) 
    BEXCEPTION("OpenSceneGraphHelper::HasDescription(): NULL argument.");
  const osg::Node::DescriptionList &dl = node->getDescriptions();
  osg::Node::DescriptionList::const_iterator it;
  for (it=dl.begin(); it!=dl.end(); it++){
    size_t pos = it->find(key);
    if (pos!=string::npos){
      value = it->substr(pos+key.size()+1, it->size()-(pos+key.size())-1);
      return true;
    }
  }
  return false;
}


bool OpenSceneGraphHelper::
RemoveInfo(osg::ref_ptr<osg::Node> node, const string &key)
{
  if (!node) 
    BEXCEPTION("OpenSceneGraphHelper::RemoveDescription(): NULL argument.");
  osg::Node::DescriptionList &dl = node->getDescriptions();
  osg::Node::DescriptionList::iterator it;
  for (it=dl.begin(); it!=dl.end(); it++){
    size_t pos = it->find(key);
    if (pos!=string::npos){
      dl.erase(it);
      return true;
    }
  }
  return false;
}


bool OpenSceneGraphHelper::
AddOrSetInfo(const osg::ref_ptr<osg::Node> node, 
             const string &key, const string &value)
{
  if (!node)
    BEXCEPTION("OpenSceneGraphHelper::AddOrSetInfo(): NULL argument.");
  bool res = true;
  string former_val;
  if (HasInfo(node, key, former_val)){
    RemoveInfo(node, key);
    res = false;
  }
  if (key.find(INFO_SEP)!=string::npos || value.find(INFO_SEP)!=string::npos){
    BEXCEPTION("OpenSceneGraphHelper::AddOrSetInfo(): Invalid character \'"
               <<INFO_SEP<<"\' in key or value.");
  }
  node->addDescription(key+INFO_SEP+value);
  return res;
}


void OpenSceneGraphHelper::
GetInfo(const osg::ref_ptr<osg::Node> node, 
        std::map<std::string, std::string> &key_val)
{
  key_val.clear();
  const osg::Node::DescriptionList &dl = node->getDescriptions();
  osg::Node::DescriptionList::const_iterator it;
  for (it=dl.begin(); it!=dl.end(); it++){
    size_t pos = it->find(INFO_SEP);
    if (pos==string::npos){
      BEXCEPTION("OpenSceneGraphHelper::GetInfo(): Invalid info (missing "
                 <<"separator \'"<<INFO_SEP<<"\'");
    }
    string key = it->substr(0, pos);
    string val = it->substr(pos+1, it->size()-(pos+1));
    key_val[key] = val;
  }  
}


osg::ref_ptr<osg::Node> OpenSceneGraphHelper::
FindNodeWithInfo(const osg::ref_ptr<osg::Node> CurrentNode, 
                 const std::string &key, const string &value)
{
  if (!CurrentNode) {
    BEXCEPTION("OpenSceneGraphHelper::FindNodeWithInfo(): NULL argument.");
  }
  string node_value;
  if (HasInfo(CurrentNode, key, node_value) && 
      (value == node_value) ){
    return CurrentNode;
  }
  const osg::ref_ptr<osg::Group> gr = CurrentNode->asGroup();
  if (gr) {
    unsigned numchildr = gr->getNumChildren();
    for(unsigned i=0; i<numchildr; i++){
      const osg::ref_ptr<osg::Node> res = 
        FindNodeWithInfo(gr->getChild(i), key, value);
      if (res) return res;
    }
  }
  return NULL;
}


std::vector<osg::ref_ptr<osg::Node> > OpenSceneGraphHelper::
FindNodesWithInfo(const osg::ref_ptr<osg::Node> CurrentNode, 
                  const std::string &key)
{
  vector<osg::ref_ptr<osg::Node> > result;
  FindNodesWithInfoHelper_(CurrentNode, key, result);
  return result;
}

 
void OpenSceneGraphHelper::
FindNodesWithInfoHelper_(const osg::ref_ptr<osg::Node> CurrentNode, 
                         const std::string &key,
                         std::vector<osg::ref_ptr<osg::Node> > &result)
{
  if (!CurrentNode) {
    BEXCEPTION("OpenSceneGraphHelper::FindNodeWithInfo(): NULL argument.");
  }
  string node_value;
  if ( HasInfo(CurrentNode, key, node_value) ){
    result.push_back(CurrentNode);
  }
  const osg::ref_ptr<osg::Group> gr = CurrentNode->asGroup();
  if (gr) {
    unsigned numchildr = gr->getNumChildren();
    for(unsigned i=0; i<numchildr; i++){
      FindNodesWithInfoHelper_(gr->getChild(i), key, result);
    }
  }
}


void OpenSceneGraphHelper::
DumpInfo(const osg::ref_ptr<osg::Node> node)
{
  const osg::Node::DescriptionList &dl = node->getDescriptions();
  osg::Node::DescriptionList::const_iterator it;
  cout << "Node \""<<node->getName()<<"\":\n";
  for (it=dl.begin(); it!=dl.end(); it++){
    cout << "  "<<*it<<endl;
  }
}


std::ostream &BIAS::operator<<(std::ostream& os, const osg::Matrix& m)
{
  os << "[";
  for (int y=0; y<4; y++){
    for (int x=0; x<4; x++){
      os << m(y,x);
      if (x!=3) os << "  ";
    }
    if (y!=3) os << ";\n";
  }
  os << "]";
  return os;

}

std::ostream &BIAS::operator<<(std::ostream& os, const osg::Vec3& v)
{
  os << "["<<v[0]<<"  "<<v[1]<<"  "<<v[2]<<"]";
  return os;
}