ThreeDOutOpenSceneGraph.cpp

#include "ThreeDOutOpenSceneGraph.hh"

#ifdef _WIN64
#  undef GL_VERSION_2_1
#endif
#include <osg/Version>
#include <osgDB/WriteFile>
#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>





//  #include <osg/Sequence>
//  #include <osg/PolygonMode>
//  #include <osg/io_utils>

#include <osgText/Font>
#include <osgText/Text>




#include <Base/Common/FileHandling.hh>
#include <Base/Image/ImageIO.hh>
#include "OpenSceneGraphHelper.hh"
#ifdef WIN32
#include <cctype>
#include <algorithm>
#endif
using namespace BIAS;
using namespace std;

ThreeDOutOpenSceneGraph::
ThreeDOutOpenSceneGraph(const BIAS::ThreeDOutParameters& params)
  : ThreeDOut(params)
{
  TexImgExtension_ = "tif";
  NewDebugLevel("D_TDOOSG_MatrixTransform");
}

ThreeDOutOpenSceneGraph::
~ThreeDOutOpenSceneGraph()
{}

int ThreeDOutOpenSceneGraph::
OpenSceneGraphOut(const std::string &filename) const
{
  //clean file name
  std::string dir;
  std::string base;
  std::string suffix, fname;
  FileHandling::SplitName(filename,dir,base,suffix);

  bool savetextures=true;

  if (suffix==".ive")
  {
    savetextures = false;
  }

  osg::ref_ptr<osg::Group> sg ;
  if(GetSceneGraph_(sg,savetextures,filename)!=0)
  {
    BIASERR("GetSceneGraph failed!");
    return(-1);
  }

  if (suffix==""){
    suffix = ".osg";
    fname = dir+base+suffix;
  } else {
    fname = filename;
  }

  osg::Node *myNode = sg.get();
  std::transform( suffix.begin(), suffix.end(),
                  suffix.begin(), (int (*)(int))std::tolower );
  BCDOUT(D_TDOOSG_MatrixTransform, "saving suffix: "<<suffix<< endl);
  osg::ref_ptr<osg::MatrixTransform> mt;
  if (suffix==".wrl"){
    BCDOUT(D_TDOOSG_MatrixTransform, "found vrml file, adding matrix "
           <<"transform for compensation\n");
    // osg wrl export automatically adds a transform node (rotation around
    // x-axis). This needs to be compensated
    mt = new osg::MatrixTransform( osg::Matrix(1, 0, 0, 0,
                                               0, 0, 1, 0,
                                               0, -1, 0, 0,
                                               0, 0, 0, 1 ));
    mt->addChild(sg.get());
    mt->setName(sg->getName());
    myNode = mt.get();
  }

  osgDB::writeNodeFile(*myNode, fname);

  return 0;
}

int ThreeDOutOpenSceneGraph::
GetOpenSceneGraph(osg::ref_ptr<osg::Group>& sg, bool exporttextures) const
{
  return(GetSceneGraph_(sg,
      exporttextures,  /*don't export textures to disc*/
                        "ThreeDOut2OSG" ));
}

int ThreeDOutOpenSceneGraph::GetSceneGraph_(osg::ref_ptr<osg::Group>& sg,
                                            bool exporttextures,
                                            const std::string &filename ) const
{
  //sg = new osg::Group();

  //set root transform node
  sg = new osg::MatrixTransform; //create transform node which is also a group
  sg->setName("RootTransform");

  osg::Node *newNode;

  AddOSGPMatrix_(sg);
  AddOSGPoints_(sg);
  AddOSGLines_(sg);
  AddOSGEllipsoids_(sg);
  AddOSGTexts_(sg);
  // graph filename is used for texture filename generation
  AddOSGImage_(sg,exporttextures,filename);
  newNode = AddOSGIndexedFaceSets_(exporttextures,filename);
  if (newNode != NULL) sg->addChild(newNode);
  return(0);
}



osg::Node* ThreeDOutOpenSceneGraph::
AddOSGIndexedFaceSets_(bool exportTextures,
                       std::string graphfilename) const
{
  if(mapIndexedFaceSet_.empty()) return NULL;

  //loop through indexed face sets
  osg::MatrixTransform* mTrans =
    new osg::MatrixTransform;
  mTrans->setName("IndexedFaceSetsTransform");

  unsigned int texcount=0;

  map<unsigned int, IndexedFaceSet>::const_iterator it;
  for(it=mapIndexedFaceSet_.begin(); it!=mapIndexedFaceSet_.end(); it++ )
  {
    //add geometry node for every face set
    //add drawable for every face set
    osg::Geode* mGeode = new osg::Geode();
#if OPENSCENEGRAPH_MAJOR_VERSION < 3 || (OPENSCENEGRAPH_MAJOR_VERSION == 3 && OPENSCENEGRAPH_MINOR_VERSION <= 1)
    osg::Geometry* mGeometry = new osg::Geometry();
#else
    deprecated_osg::Geometry* mGeometry = new deprecated_osg::Geometry();
#endif
    mGeode->addDrawable(mGeometry);
    //rootNode->addChild(mGeode);

    osg::MatrixTransform* mObjTrans =
    new osg::MatrixTransform;
    mObjTrans->setName(it->second.name+"Transform");
    mTrans->addChild(mObjTrans);
    mObjTrans->addChild(mGeode);
    mGeode->setName(it->second.name);

    //copy vertices:
    //create new vertex array
    const unsigned int vertNumb = it->second.vertexSet.size();
    osg::Vec3Array* vertices = new osg::Vec3Array(vertNumb);
    //loop through vertices
    for(unsigned int vt = 0; vt < vertNumb; vt++)
    {
      //set vertices
      const BIAS::Vector3<double> &vertexBIAS = it->second.vertexSet[vt];
      (*vertices)[vt].set(vertexBIAS[0],vertexBIAS[1],vertexBIAS[2]);
    }
    //attach vertex array to
    mGeometry->setVertexArray(vertices);

    //set texture coordinates
    if (it->second.useTexture) {
      unsigned int texCoordNumb = it->second.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++)
      {
        BIAS::Vector2<float> texCoordBIAS = it->second.texCoordSet[tc];
        (*texCoords)[tc].set(texCoordBIAS[0],texCoordBIAS[1]);
      }
      mGeometry->setTexCoordArray(0,texCoords);
    }

    //set faces:
    //loop through faces
    const unsigned int faceNumb = it->second.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 = it->second.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 = (it->second.faceIndices[fc])[ix];
        BIASASSERT(idx>=0);
        actualFace->push_back((unsigned int) idx);
      }
    }

    // set colors
    if (it->second.useColoredVertices) {
      if (it->second.vertexColorSet.size() > 0) {
        osg::Vec4Array* colours = new osg::Vec4Array();
        colours->reserve(it->second.vertexColorSet.size());

        for (size_t colourCount = 0;
             colourCount < it->second.vertexColorSet.size();
             ++colourCount) {
          const Vector4<unsigned char> &colour
              = it->second.vertexColorSet[colourCount];
          colours->push_back(osg::Vec4(float(colour[0]) / 255.0f,
                                       float(colour[1]) / 255.0f,
                                       float(colour[2]) / 255.0f,
                                       float(colour[3]) / 255.0f));
        }

        mGeometry->setColorArray(colours);
#if OPENSCENEGRAPH_MAJOR_VERSION < 3 || (OPENSCENEGRAPH_MAJOR_VERSION == 3 && OPENSCENEGRAPH_MINOR_VERSION <= 1)
        mGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
#else
        mGeometry->setColorBinding(deprecated_osg::Geometry::BIND_PER_VERTEX);
#endif
      }
    }

    // set normals
    if (it->second.useNormals && it->second.NormalArray) {
      osg::Vec3Array* normals = new osg::Vec3Array();
      normals->reserve(it->second.vertexSet.size());

      for (size_t normalCount = 0;
           normalCount < it->second.vertexSet.size();
           ++normalCount) {
        const size_t offset = normalCount * 3;

        normals->push_back(osg::Vec3(it->second.NormalArray[offset],
                                     it->second.NormalArray[offset + 1],
                                     it->second.NormalArray[offset + 2]));
      }

      mGeometry->setNormalArray(normals);
#if OPENSCENEGRAPH_MAJOR_VERSION < 3 || (OPENSCENEGRAPH_MAJOR_VERSION == 3 && OPENSCENEGRAPH_MINOR_VERSION <= 1)
      mGeometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);
#else
      mGeometry->setNormalBinding(deprecated_osg::Geometry::BIND_PER_VERTEX);
#endif
    }

    //set and save texture
    if (it->second.useTexture) {
      osg::Image*  mImage = new osg::Image;
      //mImage->setAllocationMode(osg::Image::NO_DELETE);
      string tmptexfilename = GenerateFilename_(graphfilename,
                                                it->second.name,
                                                texcount);

      // image data copying to the osgimage could be avoided for 3-4 channel
      // images if the scene graph is exported to disk only.
      // in this case the flag BIASimageToOSGimage_ could be set to "false".
      // the bias::image could be saved using bias:ImageIO.
      // this however will fail for 1 or 2 channel images at the moment thus
      // the osg::image export routines are used which requires memcpy.
      if(OpenSceneGraphHelper::BIASimageToOSGimage(it->second.texture,mImage,true /*!exportTextures*/))
      {
        if(exportTextures)
        {
          if(!osgDB::writeImageFile(*mImage,tmptexfilename))
          {
            BIASWARN("could not save texture.");
          }
          mImage->setFileName(tmptexfilename);
        }
        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
      texcount++;
    }
  }
  return mTrans;
}

std::string ThreeDOutOpenSceneGraph::
GenerateFilename_(std::string graphfilename,std::string subname,
                  unsigned int texnumber) const
{

  std::string dir;
  std::string base;
  std::string suffix;

  FileHandling::SplitName(graphfilename,dir,base,suffix);

  //add texcounter to subname
  std::stringstream sstr;
  sstr << setfill ('0')<< setw(3) << texnumber;
  return(dir+base+subname+sstr.str()+"."+TexImgExtension_);
}



void ThreeDOutOpenSceneGraph::
   AddOSGLines_(osg::ref_ptr<osg::Group>& rootNode) const
{
  if(mapLines_.empty())return;
  //add transform node
  osg::MatrixTransform* mTrans = new osg::MatrixTransform;
  mTrans->setName("LineSetTransform");
  rootNode->addChild(mTrans);

  //geometry node
  osg::Geode* mGeode = new osg::Geode;
  mGeode->setName("Lines");

  //create geometry class as drawable geometry to geometry node
#if OPENSCENEGRAPH_MAJOR_VERSION < 3 || (OPENSCENEGRAPH_MAJOR_VERSION == 3 && OPENSCENEGRAPH_MINOR_VERSION <= 1)
  osg::Geometry* mGeometry = new osg::Geometry;
#else
  deprecated_osg::Geometry* mGeometry = new deprecated_osg::Geometry;
#endif
  mGeode->addDrawable(mGeometry);
  mTrans->addChild(mGeode);

  osg::Vec3Array* vertices = new osg::Vec3Array;
  osg::Vec4ubArray* colors = new osg::Vec4ubArray;

  if (Params_.LineWidth < 1) return ;

  osg::StateSet* ss = new osg::StateSet;
  osg::LineWidth* lw     = new osg::LineWidth;
  ss->setAttribute(lw);
  //disable lighting for points
  ss->setMode( GL_LIGHTING, osg::StateAttribute::PROTECTED
                          | osg::StateAttribute::OFF );
  lw->setWidth((float)Params_.LineWidth);
  //FIXME line stipple doesn't seem to work...
  if(Params_.LineStyle==Dashed)
  {
    osg::LineStipple* ls = new osg::LineStipple;
    ls->setPattern(0x0f0f); //set 00001111 00001111 as line pattern
    ls->setFactor(5);
    ss->setAttributeAndModes(ls);
  }
  mGeode->setStateSet(ss);

  map<unsigned int, Line3DData>::const_iterator it;
  for (it=mapLines_.begin(); it!=mapLines_.end(); it++)
  {
    Vector3<double> pt1 = it->second.Start;
    Vector3<double> pt2 = it->second.End;
    RGBAuc col = it->second.Color;
    vertices->push_back(osg::Vec3(pt1[0], pt1[1], pt1[2]));
    vertices->push_back(osg::Vec3(pt2[0], pt2[1], pt2[2]));
    //bias threedout lines have only on color
    colors->push_back(osg::Vec4ub(col[0],col[1],col[2],col[3]));
  }

  mGeometry->setVertexArray(vertices);
  mGeometry->setColorArray(colors);
#if OPENSCENEGRAPH_MAJOR_VERSION < 3 || (OPENSCENEGRAPH_MAJOR_VERSION == 3 && OPENSCENEGRAPH_MINOR_VERSION <= 1)
  mGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE);
#else
  mGeometry->setColorBinding(deprecated_osg::Geometry::BIND_PER_PRIMITIVE);
#endif
  mGeometry->addPrimitiveSet(
             new osg::DrawArrays(osg::DrawArrays::LINES,0,mapLines_.size()*2));

}

void ThreeDOutOpenSceneGraph::
      AddOSGEllipsoids_(osg::ref_ptr<osg::Group>& rootNode,
                        bool camEllipsoids) const
{
  // transform node
  osg::ref_ptr<osg::MatrixTransform> mTrans = new osg::MatrixTransform;
  const std::map<unsigned int, Ellipsoid3DData>* actualElipsoidMap;
  if(camEllipsoids)
  {
    mTrans->setName("CamEllipsoidSetTransform");
    actualElipsoidMap = &mapCamEllipsoids_;
  }
  else
  {
    mTrans->setName("EllipsoidSetTransform");
    actualElipsoidMap = &mapEllipsoids_;
  }

  if(actualElipsoidMap->empty())
  {
    return;
  }

  rootNode->addChild(mTrans);

  map<unsigned int, Ellipsoid3DData>::const_iterator it;
  for(it=actualElipsoidMap->begin(); it!=actualElipsoidMap->end(); it++)
  {
    //create transform node for every ellipsoid
     osg::MatrixTransform* mTransEllip =
          new osg::MatrixTransform;
          mTrans->addChild(mTransEllip);
     osg::Matrixd transf;
     osg::Matrixd transl;
     osg::Matrixd rota; //makeRotate
     osg::Matrixd scale;
     //first scale to ellipsoid shape
     scale.makeScale(it->second.s0 * Params_.EllipsoidScale,
                     it->second.s1 * Params_.EllipsoidScale,
                     it->second.s2 * Params_.EllipsoidScale);
     rota.makeRotate(it->second.dAngle,
                     it->second.RotAxis[0],
                     it->second.RotAxis[1],
                     it->second.RotAxis[2]
                     );
     transl.makeTranslate(it->second.C[0],it->second.C[1],it->second.C[2]);

     transf = scale*rota*transl;
     mTransEllip->setMatrix(transf);

     osg::Geode* mGeode;
     osg::Sphere* mSphere = new osg::Sphere(osg::Vec3(0,0,0),1.0);
     osg::ShapeDrawable* sphereDrawable = new osg::ShapeDrawable(mSphere);
     sphereDrawable->setColor(osg::Vec4(it->second.Color[0],
                                        it->second.Color[1],
                                        it->second.Color[2],
                                        it->second.Color[3]));
     mGeode = new osg::Geode();
     mGeode->addDrawable(sphereDrawable);
     mTransEllip->addChild(mGeode);
  }
}


void ThreeDOutOpenSceneGraph::
      AddOSGPoints_(osg::ref_ptr<osg::Group>& rootNode) const
{
  if(mapPoints_.empty()) return ;
  // transform node
  osg::MatrixTransform* mTrans =
    new osg::MatrixTransform;
  mTrans->setName("PointSetTransform");
  rootNode->addChild(mTrans);

  //geometry node
  osg::ref_ptr<osg::Geode> mGeode = new osg::Geode;
  mGeode->setName("Points");
  mGeode->setCullingActive(false);

  //create geometry class as drawable geometry to geometry node
  osg::ref_ptr<osg::Geometry> mGeometry = new osg::Geometry;
  mGeode->addDrawable(mGeometry);
  mTrans->addChild(mGeode);


  osg::ref_ptr<osg::StateSet> ss = new osg::StateSet;
  osg::ref_ptr<osg::Point> p     = new osg::Point;
  // dont set the the attribute to allow client program (glutGLviewer) to set this itself, fkellner FIXME
  // ss->setAttribute(p);
  p->setSize(Params_.PointSize);
  mGeode->setStateSet(ss);

  //disable lighting for points
  ss->setMode( GL_LIGHTING, osg::StateAttribute::PROTECTED
                          | osg::StateAttribute::OFF );
  //TODO implement different pointstyles like pointsprite
  if(Params_.PointStyle != Point)
  {
     BIASWARN("unsupported PointDrawingStyle.");
  }
  // now add internally cached stuff from ThreeDOut
 //create vertex and color arrays for point data
  osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
  osg::ref_ptr<osg::Vec4ubArray> colors = new osg::Vec4ubArray;
  //loop through point data and store it in geometry class
  map<unsigned int, BIAS::Point3DData>::const_iterator it;
  for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++)
  {
    Vector3<double> pt = it->second.Point;
    RGBAuc col = it->second.Color;
    vertices->push_back(osg::Vec3(pt[0], pt[1], pt[2]));
    colors->push_back(osg::Vec4ub(col[0],col[1],col[2],col[3]));
  //  colors->push_back(osg::Vec4(((float)col[0]/255.0f),((float)col[1]/255.0f),((float)col[2]/255.0f),((float)col[3]/255.0f)));
  }
  mGeometry->setVertexArray(vertices);
  mGeometry->setColorArray(colors);
  mGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
  mGeometry->addPrimitiveSet(
             new osg::DrawArrays(osg::DrawArrays::POINTS,0,mapPoints_.size()));
}





void ThreeDOutOpenSceneGraph::
AddOSGPMatrix_(osg::ref_ptr<osg::Group>& rootNode) const
{
  if(mapPMatrices_.empty())return;
  if (Params_.CameraScale<=0.0) return;
  osg::ref_ptr<osg::MatrixTransform> mTrans =
    new osg::MatrixTransform;
  mTrans->setName("CameraSetTransform");
  rootNode->addChild(mTrans.get());
  osg::Geode* mGeode = new osg::Geode;
  mGeode->setName("Cameras");
  mTrans->addChild(mGeode);

  osg::ref_ptr<osg::Group> transgroup = mTrans.get();
  AddOSGEllipsoids_(transgroup,true);

  unsigned int idxOffset=0;


  // create state set for point parameters
  osg::StateSet* ss = new osg::StateSet;
  osg::Point* p     = new osg::Point;
  // dont set the the attribute to allow client program (glutGLviewer) to set this itself, fkellner FIXME
  //ss->setAttribute(p);
  p->setSize(Params_.PointSize);

  //set line width
  osg::LineWidth* lw     = new osg::LineWidth;
  // dont set the the attribute to allow client program (glutGLviewer) to set this itself, fkellner FIXME
  //ss->setAttribute(lw);
  //disable lighting for points
  ss->setMode( GL_LIGHTING, osg::StateAttribute::PROTECTED
                          | osg::StateAttribute::OFF );
  lw->setWidth((float)Params_.LineWidth);
  mGeode->setStateSet(ss);
  if(Params_.LineStyle==Dashed)
  {
    osg::LineStipple* ls = new osg::LineStipple;
    ls->setPattern(0x0f0f); //set 00001111 00001111 as line pattern
    ls->setFactor(128);
    ss->setAttributeAndModes(ls);
  }


  map<unsigned int, PMatrix3DData>::const_iterator it;
  for (it=mapPMatrices_.begin(); it!=mapPMatrices_.end(); it++)
  {
    //create geometry class as drawable geometry to geometry node
    osg::Geometry* mGeometry = new osg::Geometry;
    mGeode->addDrawable(mGeometry);
    osg::DrawElementsUInt* drawElmtsU=NULL;

    if(Params_.CameraStyle!=PyramidSolid)
    {
      drawElmtsU = new osg::DrawElementsUInt(osg::PrimitiveSet::LINES);
    }
    else
    {
      drawElmtsU = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
    }

    //create index set for points
    osg::DrawElementsUInt* drawElmtsPointsU=
         new osg::DrawElementsUInt(osg::PrimitiveSet::POINTS);

    const PMatrix3DData *i = &(it->second);
    mGeometry->setName(i->name);

    //the camera center is needed in all render cases
    osg::Vec3Array* vertices = new osg::Vec3Array;
    osg::Vec4ubArray* colors = new osg::Vec4ubArray;

    osg::Vec3 vC  = osg::Vec3(i->C[0],i->C[1],i->C[2]);
    vertices->push_back(vC) ;//center has index 0
    RGBAuc col = i->Color;
    // color for the point
    colors->push_back(osg::Vec4ub(col[0],col[1],col[2],col[3]));
    // color for the line strip/triangles etc.
    colors->push_back(osg::Vec4ub(col[0],col[1],col[2],col[3]));
    idxOffset = 1;

    drawElmtsPointsU->push_back(0); //add center as point

    //add the pyramid vertices to vertex list
    if(Params_.CameraStyle!=Arrow)
    {

        osg::Vec3 vUR = osg::Vec3(i->UR[0],i->UR[1],i->UR[2]);
        osg::Vec3 vUL = osg::Vec3(i->UL[0],i->UL[1],i->UL[2]);
        osg::Vec3 vLL = osg::Vec3(i->LL[0],i->LL[1],i->LL[2]);
        osg::Vec3 vLR = osg::Vec3(i->LR[0],i->LR[1],i->LR[2]);

        //scaling : move to 0,0,0, Params_.CameraScale, move back to center
        vUR = ((vUR - vC) * Params_.CameraScale) + vC ;
        vUL = ((vUL - vC) * Params_.CameraScale) + vC ;
        vLL = ((vLL - vC) * Params_.CameraScale) + vC ;
        vLR = ((vLR - vC) * Params_.CameraScale) + vC ;

        vertices->push_back(vUR);//1
        vertices->push_back(vUL);//2
        vertices->push_back(vLL);//3
        vertices->push_back(vLR);//4
        idxOffset = 5;

        // add sphere at top letf image corner
        osg::ref_ptr<osg::MatrixTransform> mTrans = new osg::MatrixTransform();
        osg::Matrixd identitymat;
        identitymat.makeIdentity();
        mTrans->setMatrix(identitymat);
        rootNode->addChild(mTrans);
        osg::Geode* mGeode;
        osg::Sphere* mSphere = new osg::Sphere(osg::Vec3(vUL[0],vUL[1],vUL[2]),
            Params_.CameraScale/100.0);
        osg::TessellationHints *fewfaces = new osg::TessellationHints;
        fewfaces->setTargetNumFaces(4);
        fewfaces->setTessellationMode(osg::TessellationHints::USE_TARGET_NUM_FACES);


        osg::ShapeDrawable* sphereDrawable = new osg::ShapeDrawable(mSphere,fewfaces);
        sphereDrawable->setColor(osg::Vec4f(float(col[0]),
                                 float(col[1]),
            float(col[2]),
            float(col[3])));

        mGeode = new osg::Geode();
        mGeode->addDrawable(sphereDrawable);
        mTrans->addChild(mGeode);

    }

    switch (Params_.CameraStyle)
    {
      case PyramidMeshWithArrow:
      case PyramidMesh:
      case PyramidWithCenterBox:
      {
        //draw lines of pyramid
        //add line indices
        drawElmtsU->push_back(0);drawElmtsU->push_back(1);
        drawElmtsU->push_back(0);drawElmtsU->push_back(2);
        drawElmtsU->push_back(0);drawElmtsU->push_back(3);
        drawElmtsU->push_back(0);drawElmtsU->push_back(4);
        drawElmtsU->push_back(1);drawElmtsU->push_back(2);
        drawElmtsU->push_back(2);drawElmtsU->push_back(3);
        drawElmtsU->push_back(3);drawElmtsU->push_back(4);
        drawElmtsU->push_back(4);drawElmtsU->push_back(1);
        //draw no arrow? break!
        if (Params_.CameraStyle==PyramidMesh)  break; //no point for mesh...
      }
      case Arrow:
      {
        if (Params_.LineWidth < 1) break;
        //draw arrow
        if (Params_.CameraStyle==PyramidMeshWithArrow
          ||Params_.CameraStyle==Arrow)
        {
          osg::Vec3 vPP = osg::Vec3(i->PP[0],i->PP[1],i->PP[2]);
          vertices->push_back(vPP); //idxOffset + 0;
          osg::Vec3 vUPC = osg::Vec3(i->C[0] ,i->C[1] ,i->C[2])+
                           osg::Vec3(i->Up[0],i->Up[1],i->Up[2]);
          vertices->push_back(vUPC);
          drawElmtsU->push_back(0);drawElmtsU->push_back(idxOffset);
          drawElmtsU->push_back(0);drawElmtsU->push_back(idxOffset+1);
        }
        break;
      }
      case PyramidSolid:
      {
        drawElmtsU->push_back(0);drawElmtsU->push_back(1);
        drawElmtsU->push_back(2);
        drawElmtsU->push_back(0);drawElmtsU->push_back(3);
        drawElmtsU->push_back(4);
        drawElmtsU->push_back(0);drawElmtsU->push_back(4);
        drawElmtsU->push_back(1);
        drawElmtsU->push_back(0);drawElmtsU->push_back(2);
        drawElmtsU->push_back(3);
        drawElmtsU->push_back(1);drawElmtsU->push_back(3);
        drawElmtsU->push_back(2);
        drawElmtsU->push_back(1);drawElmtsU->push_back(4);
        drawElmtsU->push_back(3);
        break;
      }
      default:
      {
        BIASERR("unexpected pmatrix draw style");
        BIASABORT;
      }
    } //switch
    mGeometry->setVertexArray(vertices);
    mGeometry->setColorArray(colors);
    mGeometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE_SET);
    mGeometry->addPrimitiveSet(drawElmtsPointsU);
    mGeometry->addPrimitiveSet(drawElmtsU);
  } //loop through pmatrices
}


void ThreeDOutOpenSceneGraph::
   AddOSGImage_(osg::ref_ptr<osg::Group>& rootNode,
                bool exportTextures,
                std::string graphfilename) const
{
  if(mapImages_.empty())return;
   //billboarding in osg?
   //http://www.openscenegraph.org/documentation/NPSTutorials/osgBillboard.htm
   //http://www.openscenegraph.org/documentation/OpenSceneGraphReferenceDocs/a01041.html
  unsigned int texcount=0;
  map<unsigned int, Image3DData>::const_iterator it;
  for (it = mapImages_.begin(); it!=mapImages_.end(); it++)
  {

    osg::Geometry* mGeometry = new osg::Geometry();
    osg::Geode* mGeode = 0;
    osg::Billboard* mBB = 0;
    if(it->second.BillBoard)
    {
       // set billboard rotation
        mBB = new osg::Billboard();
        rootNode->addChild(mBB);
        mBB->setMode(osg::Billboard::AXIAL_ROT); //POINT_ROT_EYE POINT_ROT_WORLD
        mBB->setAxis(osg::Vec3(0.0f,1.0f,0.0f));
        mBB->setNormal(osg::Vec3(0.0f,0.0f,1.0f));

        rootNode->addChild(mBB);
        mBB->setName(it->second.name);
    }
    else  // not a billboard
    {
       mGeode = new osg::Geode();
       mGeode->addDrawable(mGeometry);
       rootNode->addChild(mGeode);
       mGeode->setName(it->second.name);
    }

    //create new vertex array
    osg::Vec3Array* vertices = new osg::Vec3Array(4);
    if(!it->second.BillBoard)
    {
       (*vertices)[0].set(it->second.UL[0],it->second.UL[1],it->second.UL[2]);
       (*vertices)[1].set(it->second.LL[0],it->second.LL[1],it->second.LL[2]);
       (*vertices)[2].set(it->second.LR[0],it->second.LR[1],it->second.LR[2]);
       (*vertices)[3].set(it->second.UR[0],it->second.UR[1],it->second.UR[2]);
    }
    else
    {
       //adjust vertices for billboard usage
       BIAS::Vector3<double> bbcenter = it->second.UL + it->second.LR ;
       bbcenter.MultIP(0.5); //get center of image

       (*vertices)[0].set(it->second.UL[0]-bbcenter[0],
                          it->second.UL[1]-bbcenter[1],
                          it->second.UL[2]-bbcenter[2]);
       (*vertices)[1].set(it->second.LL[0]-bbcenter[0],
                          it->second.LL[1]-bbcenter[1]
                         ,it->second.LL[2]-bbcenter[2]);
       (*vertices)[2].set(it->second.LR[0]-bbcenter[0],
                          it->second.LR[1]-bbcenter[1],
                          it->second.LR[2]-bbcenter[2]);
       (*vertices)[3].set(it->second.UR[0]-bbcenter[0],
                          it->second.UR[1]-bbcenter[1],
                          it->second.UR[2]-bbcenter[2]);
      /* To help place billboards there is an overload to
         osg::Geode's addGeometry method. This method takes two arguments:
         a drawable instance and an osg::Vec3 instance for position.*/
      mBB->addDrawable( mGeometry, osg::Vec3(bbcenter[0],
                                             bbcenter[1],
                                             bbcenter[2]));
    }
      //attach vertex array to
      mGeometry->setVertexArray(vertices);

      osg::DrawElementsUInt*  actualFace;
      actualFace =  new osg::DrawElementsUInt(osg::PrimitiveSet::QUADS);
      mGeometry->addPrimitiveSet(actualFace);
      actualFace->push_back((unsigned int) 0);
      actualFace->push_back((unsigned int) 1);
      actualFace->push_back((unsigned int) 2);
      actualFace->push_back((unsigned int) 3);

      //texture coordinates
      osg::Vec2Array* texCoords = new osg::Vec2Array(4);
      (*texCoords)[0].set(0,1);
      (*texCoords)[1].set(0,0);
      (*texCoords)[2].set(1,0);
      (*texCoords)[3].set(1,1);
      mGeometry->setTexCoordArray(0,texCoords);

      //texture
      //set and save texture
      osg::Image*  mImage = new osg::Image;
      string tmptexfilename = GenerateFilename_(graphfilename,
           string("img")+it->second.name,texcount);

      osg::StateSet *ss = new osg::StateSet;

      if(OpenSceneGraphHelper::BIASimageToOSGimage(it->second.ImageData,mImage,!exportTextures))
      {
        if(exportTextures)
        {
          if(ImageIO::Save(tmptexfilename,it->second.ImageData)==-1)
          {
            BIASWARN("could not save texture.");
          }
        }
        mImage->setFileName(tmptexfilename);
        osg::ref_ptr< osg::Texture2D > mTex2D = new osg::Texture2D;
        mTex2D->setImage( mImage );


        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);
      }
     if(it->second.BillBoard)
     {
        mBB->setStateSet(ss);
     }
     else
     {
       mGeode->setStateSet(ss);
     }
      texcount++;
  }//for

}


void ThreeDOutOpenSceneGraph::AddOSGTexts_(osg::ref_ptr<osg::Group>& rootNode) const {

  if(mapTexts_.empty()) return ;
  // transform node
  osg::MatrixTransform* mTrans = new osg::MatrixTransform;
  mTrans->setName("TextTransform");
  rootNode->addChild(mTrans);

  //loop through text data
  map<unsigned int, BIAS::Text3DData>::const_iterator it;
  for (it=mapTexts_.begin(); it!=mapTexts_.end(); it++) {

    osg::ref_ptr<osg::Geode> mGeode = new osg::Geode;
    mGeode->setName("Text");
    mGeode->setCullingActive(false);
    mTrans->addChild(mGeode);

    osgText::Text* text4 = new osgText::Text;
    text4->setFont("fonts/times.ttf");

    if (it->second.Size>0.0)
      text4->setCharacterSize(it->second.Size);
    else {
      double thesize = -1.0 * it->second.Size;
      if (thesize<5) thesize = 5;
      text4->setCharacterSizeMode(osgText::Text::OBJECT_COORDS_WITH_MAXIMUM_SCREEN_SIZE_CAPPED_BY_FONT_HEIGHT);
      text4->setCharacterSize(thesize);
    }
    text4->setPosition(osg::Vec3(it->second.Position[0],
                       it->second.Position[1],
        it->second.Position[2]));



    if (it->second.Direction.NormL2()>0.0) {
      Quaternion<double> q = it->second.Direction;
      /*
      Vector3<double> dir
      dir.Normalize();
      cout<<"dir is "<<dir<<endl;
      RMatrix R;

      Vector3<double> ydir(0.0,1.0,0.0), zdir(0.0,0.0,1.0);
      if (fabs(dir.ScalarProduct(ydir) < fabs(dir.ScalarProduct(zdir)))) {
        cout<<"y is more orthogonal to x than z is"<<endl;
        zdir = dir.CrossProduct(ydir);
        zdir.Normalize();
        ydir = zdir.CrossProduct(dir);
      } else {
        cout<<"z is more orthogonal to x than y is"<<endl;
        ydir = zdir.CrossProduct(dir);
        ydir.Normalize();
        zdir = dir.CrossProduct(ydir);
      }
      for (unsigned int i=0; i<3; i++) {
        R[i][0] = dir[i];
        R[i][1] = ydir[i];
        R[i][2] = zdir[i];
      }
      cout<<"R is "<<R<<" with RTR = "<<R*R.Transpose()<<endl;
      Quaternion<double> q;
      R.GetQuaternion(q);
      */
      text4->setRotation(osg::Quat(q[0], q[1], q[2], q[3]));
    } else {
      text4->setAxisAlignment(osgText::Text::SCREEN);
      text4->setAlignment(osgText::Text::CENTER_CENTER);
    }
    // text4->setAxisAlignment(osgText::Text::XZ_PLANE);
    // reproduce outline bounding box compute problem with backdrop on.
    text4->setBackdropType(osgText::Text::OUTLINE);
    //text4->setDrawMode(osgText::Text::TEXT | osgText::Text::BOUNDINGBOX);

    text4->setColor(osg::Vec4(float(it->second.Color[0])/255.0, float(it->second.Color[1])/255.0,
        float(it->second.Color[2])/255.0,float(it->second.Color[3])/255.0));
    text4->setText(it->second.Text);
    mGeode->addDrawable(text4);

  }
}