vrml_output.h

Go to the documentation of this file.

// HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
// Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
//
// This file is part of HOG-Man.
// 
// HOG-Man is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// HOG-Man is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.

#ifndef VRML_OUTPUT_H
#define VRML_OUTPUT_H

// @{

// TODO implement an vrml equivalent for all the other translate rotate functions of OpenGL
// TODO needs implementation and testing of glScalef for the vrml matrix stack

#include "matrix4x4.h"
#include "stuff/macros.h"

// forward declarations
namespace qglviewer {
  class Vec;
  class Quaternion;
}

#include <iostream>
#include <iterator>
#include <iomanip>

// vrml color
typedef union {
  struct {
    float r; 
    float g; 
    float b; 
    float alpha; 
  }; 
  float color[4]; 
  float operator[] (int i) const {return color[i];}
  float& operator[] (int i) {return color[i];}
} vrml_color_t;

union vrml_point_t {
  struct {
    double x; 
    double y; 
    double z; 
  };
  double coords[3]; 
  double operator[] (int i) const {return coords[i];}
  double& operator[] (int i) {return coords[i];}
  vrml_point_t() : x(0.), y(0.), z(0.) {}
  vrml_point_t(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
};
std::ostream& operator<<(std::ostream& os, const vrml_point_t& p);

struct vrml_colored_point_t {
  vrml_point_t point;
  vrml_color_t color;
  vrml_colored_point_t() {color[3] = 1.0;}
}; 

extern vrml_color_t g_vrml_color;
extern vrml::Matrix4x4 g_vrml_modelview;

void vrmlHeadTranslation(qglviewer::Vec& p);

void vrmlHeadRotation(qglviewer::Quaternion& quat);

void vrmlColor3f(float r, float g, float b);

void vrmlColor4f(float r, float g, float b, float alpha);

void vrmlRotatef(float phi, float x, float y, float z);

void vrmlTranslatef(float dx, float dy, float dz);

void vrmlScalef(float sx, float sy, float sz);

void vrmlMultMatrix(const vrml::Matrix4x4& m);

void vrmlPushMatrix();

void vrmlPopMatrix();

void vrmlLoadIdentity();

void writeVrmlFileHeader(std::ostream& os);

void writeStart(std::ostream& os);

void writeEnd(std::ostream& os);

void writeCurrentHead(std::ostream& os, bool allColor = false);

void writeCurrentFoot(std::ostream& os);

void writeScaleHead(std::ostream& os, double sx, double sy, double sz);

void writeScaleFoot(std::ostream& os);

void writeBox(std::ostream& os, double l, double w, double h);

void writePlane(std::ostream& os, double l, double w);

void writeSphere(std::ostream& os, double radius);

void writeEllipsoid(std::ostream& os, double r1, double r2, double r3);

void writeCone(std::ostream& os, double radius, double height);

void writeCylinder(std::ostream& os, double radius, double height);

void writePyramid(std::ostream& os, double length, double height);

void writeSlice(std::ostream& os, double radius, double height, double fov, int slices_per_circle = 32);

void writeAxes(std::ostream&os, double len=1.0);

template <class InputIterator>
void writeLines(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os);
  os << "IndexedLineSet {\n"
     << "  color Color {\n"
     << "    color [\n"
     << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
     << "    ]\n"
     << "  }\n"
     << "  coord Coordinate {\n" << "    point [\n";
  unsigned int cnt = 0;
  for (InputIterator it = begin; it != end; ++it) {
    os << FIXED((*it)[0] << " " << (*it)[1] << " " << (*it)[2]) << ",\n";
    cnt++;
  }
  os << "    ]\n" << "  }\n";
  os << "  colorIndex [\n";
  for (unsigned int i = 0; i < cnt; i+=2) {
    os << "0 0 -1\n";
  }
  os << "\n  ]\n";
  os << "  coordIndex [\n";
  for (unsigned int i = 0; i < cnt; ) {
    os << i++ << " ";
    os << i++ << " -1\n";
  }
  os << "  ]\n"
     << "}\n";
  writeCurrentFoot(os);
}

template <class InputIterator>
void writeLineStrip(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os);
  os << "IndexedLineSet {\n"
     << "  color Color {\n"
     << "    color [\n"
     << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
     << "    ]\n"
     << "  }\n"
     << "  coord Coordinate {\n" << "    point [\n";
  unsigned int cnt = 0;
  for (InputIterator it = begin; it != end; ++it) {
    os << FIXED((*it)[0] << " " << (*it)[1] << " " << (*it)[2]) << ",\n";
    cnt++;
  }
  os << "    ]\n" << "  }\n";
  os << "  colorIndex [\n";
  for (unsigned int i = 0; i < cnt; i++)
    os << "0 ";
  os << "-1\n";
  os << "\n  ]\n";
  os << "  coordIndex [\n";
  for (unsigned int i = 0; i < cnt; ) {
    os << i++ << " ";
  }
  os << "-1\n";
  os << "  ]\n"
     << "}\n";
  writeCurrentFoot(os);
}

template <class InputIterator>
void writeTriangles(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os);
  os << "IndexedFaceSet {\n"
     << "  solid FALSE\n"
     << "  coord Coordinate {\n"
     << "    point [\n";
  unsigned int cnt = 0;
  for (InputIterator it = begin; it != end; ++it) {
    os << FIXED((*it)[0] << " " <<  (*it)[1] << " " << (*it)[2]) << std::endl;
    ++cnt;
  }
  os << "    ]\n"
     << "  }\n"
     << "  coordIndex  [\n";
  for (unsigned int i = 0; i < cnt; ) {
    os << i++ << " ";
    os << i++ << " ";
    os << i++ << " -1\n";
  }
  os << "  ]\n"
     << "}\n";
  writeCurrentFoot(os);
}

template <class InputIterator>
void writeQuads(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os);
  os << "IndexedFaceSet {\n"
     << "  solid FALSE\n"
     << "  coord Coordinate {\n"
     << "    point [\n";
  unsigned int cnt = 0;
  for (InputIterator it = begin; it != end; ++it) {
    os << FIXED((*it)[0] << " " <<  (*it)[1] << " "  << (*it)[2]) << "\n";
    ++cnt;
  }
  os << "    ]\n"
     << "  }\n"
     << "  coordIndex  [\n";
  for (unsigned int i = 0; i < cnt; ) {
    os << i++ << " ";
    os << i++ << " ";
    os << i++ << " ";
    os << i++ << " -1\n";
  }
  os << "  ]\n"
     << "}\n";
  writeCurrentFoot(os);
}

template <class InputIterator>
void writePoints(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os, true);
  os << "PointSet {\n";
  os << "  coord Coordinate {\n";
  os << "    point [\n";
  std::streamsize oldPrec = os.precision();
  os << std::fixed << std::setprecision(3);
  for (InputIterator it = begin; it != end; ++it)
    os << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << "\n";
  os << std::resetiosflags(std::ios_base::floatfield);
  os.precision(oldPrec);
  os << "    ]\n";
  os << "  }\n";
  os << "}\n";
  writeCurrentFoot(os);
}

template <class InputIterator>
void writeColoredPoints(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os);
  os << "PointSet {\n";
  os << " color Color {\n";
  os << "   color [\n";
  for (InputIterator it = begin; it != end; ++it)
    os << (*it).color[0] << " " << (*it).color[1] << " " << (*it).color[2] << "\n";
  os << "   ]\n";
  os << "  }\n";
  os << "  coord Coordinate {\n";
  os << "    point [\n";
  for (InputIterator it = begin; it != end; ++it)
    os << FIXED((*it).point[0] << " " << (*it).point[1] << " " << (*it).point[2]) << "\n";
  os << "    ]\n";
  os << "  }\n";
  os << "}\n";
  writeCurrentFoot(os);
}

template <class InputIterator>
void writeColoredLines(std::ostream& os, const InputIterator& begin, const InputIterator& end)
{
  writeCurrentHead(os);
  os << "IndexedLineSet {\n"
    << "  color Color {\n"
    << "    color [\n";
  unsigned int cnt = 0;
  for (InputIterator it = begin; it != end; ++it, ++cnt)
    os << (*it).color[0] << " " << (*it).color[1] << " " << (*it).color[2] << "\n";
  os << "    ]\n"
    << "  }\n"
    << "  coord Coordinate {\n" << "    point [\n";
  for (InputIterator it = begin; it != end; ++it)
    os << FIXED((*it).point[0] << " " << (*it).point[1] << " " << (*it).point[2]) << ",\n";
  os << "    ]\n" << "  }\n";
  os << "  colorIndex [\n";
  for (unsigned int i = 0; i < cnt; ) {
    os << i++ << " ";
    os << i++ << " -1\n";
  }
  os << "\n  ]\n";
  os << "  coordIndex [\n";
  for (unsigned int i = 0; i < cnt; ) {
    os << i++ << " ";
    os << i++ << " -1\n";
  }
  os << "  ]\n"
    << "}\n";
  writeCurrentFoot(os);
}

template <class MatrixType>
void setVrmlTransform(const MatrixType& mat)
{
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j)
      g_vrml_modelview[i][j] = mat[i][j];
}

void writePoseBox(std::ostream& os);

// @}

#endif