hogman_minimal: hogman_minimal: vrml

00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // This file is part of HOG-Man.
00005 // 
00006 // HOG-Man is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // (at your option) any later version.
00010 // 
00011 // HOG-Man is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 // 
00016 // You should have received a copy of the GNU General Public License
00017 // along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #ifndef VRML_OUTPUT_H
00020 #define VRML_OUTPUT_H
00021 
00023 // @{
00024 
00036 // TODO implement an vrml equivalent for all the other translate rotate functions of OpenGL
00037 // TODO needs implementation and testing of glScalef for the vrml matrix stack
00038 
00039 #include "matrix4x4.h"
00040 #include "stuff/macros.h"
00041 
00042 // forward declarations
00043 namespace qglviewer {
00044   class Vec;
00045   class Quaternion;
00046 }
00047 
00048 #include <iostream>
00049 #include <iterator>
00050 #include <iomanip>
00051 
00052 // vrml color
00056 typedef union {
00057   struct {
00058     float r; 
00059     float g; 
00060     float b; 
00061     float alpha; 
00062   }; 
00063   float color[4]; 
00064   float operator[] (int i) const {return color[i];}
00065   float& operator[] (int i) {return color[i];}
00066 } vrml_color_t;
00067 
00071 union vrml_point_t {
00072   struct {
00073     double x; 
00074     double y; 
00075     double z; 
00076   };
00077   double coords[3]; 
00078   double operator[] (int i) const {return coords[i];}
00079   double& operator[] (int i) {return coords[i];}
00080   vrml_point_t() : x(0.), y(0.), z(0.) {}
00081   vrml_point_t(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
00082 };
00083 std::ostream& operator<<(std::ostream& os, const vrml_point_t& p);
00084 
00088 struct vrml_colored_point_t {
00089   vrml_point_t point;
00090   vrml_color_t color;
00091   vrml_colored_point_t() {color[3] = 1.0;}
00092 }; 
00093 
00095 extern vrml_color_t g_vrml_color;
00097 extern vrml::Matrix4x4 g_vrml_modelview;
00098 
00102 void vrmlHeadTranslation(qglviewer::Vec& p);
00103 
00107 void vrmlHeadRotation(qglviewer::Quaternion& quat);
00108 
00112 void vrmlColor3f(float r, float g, float b);
00113 
00117 void vrmlColor4f(float r, float g, float b, float alpha);
00118 
00124 void vrmlRotatef(float phi, float x, float y, float z);
00125 
00129 void vrmlTranslatef(float dx, float dy, float dz);
00130 
00134 void vrmlScalef(float sx, float sy, float sz);
00135 
00139 void vrmlMultMatrix(const vrml::Matrix4x4& m);
00140 
00144 void vrmlPushMatrix();
00145 
00150 void vrmlPopMatrix();
00151 
00155 void vrmlLoadIdentity();
00156 
00161 void writeVrmlFileHeader(std::ostream& os);
00162 
00166 void writeStart(std::ostream& os);
00167 
00171 void writeEnd(std::ostream& os);
00172 
00177 void writeCurrentHead(std::ostream& os, bool allColor = false);
00178 
00183 void writeCurrentFoot(std::ostream& os);
00184 
00188 void writeScaleHead(std::ostream& os, double sx, double sy, double sz);
00189 
00193 void writeScaleFoot(std::ostream& os);
00194 
00202 void writeBox(std::ostream& os, double l, double w, double h);
00203 
00210 void writePlane(std::ostream& os, double l, double w);
00211 
00217 void writeSphere(std::ostream& os, double radius);
00218 
00226 void writeEllipsoid(std::ostream& os, double r1, double r2, double r3);
00227 
00231 void writeCone(std::ostream& os, double radius, double height);
00232 
00239 void writeCylinder(std::ostream& os, double radius, double height);
00240 
00244 void writePyramid(std::ostream& os, double length, double height);
00245 
00254 void writeSlice(std::ostream& os, double radius, double height, double fov, int slices_per_circle = 32);
00255 
00261 void writeAxes(std::ostream&os, double len=1.0);
00262 
00268 template <class InputIterator>
00269 void writeLines(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00270 {
00271   writeCurrentHead(os);
00272   os << "IndexedLineSet {\n"
00273      << "  color Color {\n"
00274      << "    color [\n"
00275      << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
00276      << "    ]\n"
00277      << "  }\n"
00278      << "  coord Coordinate {\n" << "    point [\n";
00279   unsigned int cnt = 0;
00280   for (InputIterator it = begin; it != end; ++it) {
00281     os << FIXED((*it)[0] << " " << (*it)[1] << " " << (*it)[2]) << ",\n";
00282     cnt++;
00283   }
00284   os << "    ]\n" << "  }\n";
00285   os << "  colorIndex [\n";
00286   for (unsigned int i = 0; i < cnt; i+=2) {
00287     os << "0 0 -1\n";
00288   }
00289   os << "\n  ]\n";
00290   os << "  coordIndex [\n";
00291   for (unsigned int i = 0; i < cnt; ) {
00292     os << i++ << " ";
00293     os << i++ << " -1\n";
00294   }
00295   os << "  ]\n"
00296      << "}\n";
00297   writeCurrentFoot(os);
00298 }
00299 
00305 template <class InputIterator>
00306 void writeLineStrip(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00307 {
00308   writeCurrentHead(os);
00309   os << "IndexedLineSet {\n"
00310      << "  color Color {\n"
00311      << "    color [\n"
00312      << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
00313      << "    ]\n"
00314      << "  }\n"
00315      << "  coord Coordinate {\n" << "    point [\n";
00316   unsigned int cnt = 0;
00317   for (InputIterator it = begin; it != end; ++it) {
00318     os << FIXED((*it)[0] << " " << (*it)[1] << " " << (*it)[2]) << ",\n";
00319     cnt++;
00320   }
00321   os << "    ]\n" << "  }\n";
00322   os << "  colorIndex [\n";
00323   for (unsigned int i = 0; i < cnt; i++)
00324     os << "0 ";
00325   os << "-1\n";
00326   os << "\n  ]\n";
00327   os << "  coordIndex [\n";
00328   for (unsigned int i = 0; i < cnt; ) {
00329     os << i++ << " ";
00330   }
00331   os << "-1\n";
00332   os << "  ]\n"
00333      << "}\n";
00334   writeCurrentFoot(os);
00335 }
00336 
00341 template <class InputIterator>
00342 void writeTriangles(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00343 {
00344   writeCurrentHead(os);
00345   os << "IndexedFaceSet {\n"
00346      << "  solid FALSE\n"
00347      << "  coord Coordinate {\n"
00348      << "    point [\n";
00349   unsigned int cnt = 0;
00350   for (InputIterator it = begin; it != end; ++it) {
00351     os << FIXED((*it)[0] << " " <<  (*it)[1] << " " << (*it)[2]) << std::endl;
00352     ++cnt;
00353   }
00354   os << "    ]\n"
00355      << "  }\n"
00356      << "  coordIndex  [\n";
00357   for (unsigned int i = 0; i < cnt; ) {
00358     os << i++ << " ";
00359     os << i++ << " ";
00360     os << i++ << " -1\n";
00361   }
00362   os << "  ]\n"
00363      << "}\n";
00364   writeCurrentFoot(os);
00365 }
00366 
00371 template <class InputIterator>
00372 void writeQuads(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00373 {
00374   writeCurrentHead(os);
00375   os << "IndexedFaceSet {\n"
00376      << "  solid FALSE\n"
00377      << "  coord Coordinate {\n"
00378      << "    point [\n";
00379   unsigned int cnt = 0;
00380   for (InputIterator it = begin; it != end; ++it) {
00381     os << FIXED((*it)[0] << " " <<  (*it)[1] << " "  << (*it)[2]) << "\n";
00382     ++cnt;
00383   }
00384   os << "    ]\n"
00385      << "  }\n"
00386      << "  coordIndex  [\n";
00387   for (unsigned int i = 0; i < cnt; ) {
00388     os << i++ << " ";
00389     os << i++ << " ";
00390     os << i++ << " ";
00391     os << i++ << " -1\n";
00392   }
00393   os << "  ]\n"
00394      << "}\n";
00395   writeCurrentFoot(os);
00396 }
00397 
00402 template <class InputIterator>
00403 void writePoints(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00404 {
00405   writeCurrentHead(os, true);
00406   os << "PointSet {\n";
00407   os << "  coord Coordinate {\n";
00408   os << "    point [\n";
00409   std::streamsize oldPrec = os.precision();
00410   os << std::fixed << std::setprecision(3);
00411   for (InputIterator it = begin; it != end; ++it)
00412     os << (*it)[0] << " " << (*it)[1] << " " << (*it)[2] << "\n";
00413   os << std::resetiosflags(std::ios_base::floatfield);
00414   os.precision(oldPrec);
00415   os << "    ]\n";
00416   os << "  }\n";
00417   os << "}\n";
00418   writeCurrentFoot(os);
00419 }
00420 
00425 template <class InputIterator>
00426 void writeColoredPoints(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00427 {
00428   writeCurrentHead(os);
00429   os << "PointSet {\n";
00430   os << " color Color {\n";
00431   os << "   color [\n";
00432   for (InputIterator it = begin; it != end; ++it)
00433     os << (*it).color[0] << " " << (*it).color[1] << " " << (*it).color[2] << "\n";
00434   os << "   ]\n";
00435   os << "  }\n";
00436   os << "  coord Coordinate {\n";
00437   os << "    point [\n";
00438   for (InputIterator it = begin; it != end; ++it)
00439     os << FIXED((*it).point[0] << " " << (*it).point[1] << " " << (*it).point[2]) << "\n";
00440   os << "    ]\n";
00441   os << "  }\n";
00442   os << "}\n";
00443   writeCurrentFoot(os);
00444 }
00445 
00452 template <class InputIterator>
00453 void writeColoredLines(std::ostream& os, const InputIterator& begin, const InputIterator& end)
00454 {
00455   writeCurrentHead(os);
00456   os << "IndexedLineSet {\n"
00457     << "  color Color {\n"
00458     << "    color [\n";
00459   unsigned int cnt = 0;
00460   for (InputIterator it = begin; it != end; ++it, ++cnt)
00461     os << (*it).color[0] << " " << (*it).color[1] << " " << (*it).color[2] << "\n";
00462   os << "    ]\n"
00463     << "  }\n"
00464     << "  coord Coordinate {\n" << "    point [\n";
00465   for (InputIterator it = begin; it != end; ++it)
00466     os << FIXED((*it).point[0] << " " << (*it).point[1] << " " << (*it).point[2]) << ",\n";
00467   os << "    ]\n" << "  }\n";
00468   os << "  colorIndex [\n";
00469   for (unsigned int i = 0; i < cnt; ) {
00470     os << i++ << " ";
00471     os << i++ << " -1\n";
00472   }
00473   os << "\n  ]\n";
00474   os << "  coordIndex [\n";
00475   for (unsigned int i = 0; i < cnt; ) {
00476     os << i++ << " ";
00477     os << i++ << " -1\n";
00478   }
00479   os << "  ]\n"
00480     << "}\n";
00481   writeCurrentFoot(os);
00482 }
00483 
00487 template <class MatrixType>
00488 void setVrmlTransform(const MatrixType& mat)
00489 {
00490   for (int i = 0; i < 4; ++i)
00491     for (int j = 0; j < 4; ++j)
00492       g_vrml_modelview[i][j] = mat[i][j];
00493 }
00494 
00498 void writePoseBox(std::ostream& os);
00499 
00500 // @}
00501 
00502 #endif
vrml_output.h
