vrml_output.cpp
Go to the documentation of this file.
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 #include "vrml_output.h"
00020 
00021 #include "axes.h"
00022 #include "stuff/macros.h"
00023 #include <qglviewer/vec.h>
00024 #include <qglviewer/quaternion.h>
00025 
00026 #include <list>
00027 #include <cmath>
00028 #include <stack>
00029 #include <stdexcept>
00030 #include <limits>
00031 using namespace std;
00032 //using namespace vrml;
00033 
00034 vrml_color_t g_vrml_color = { {0, 0, 0, 1} };
00035 vrml::Matrix4x4 g_vrml_modelview = vrml::Matrix4x4::identity();
00036 
00037 static stack<vrml::Matrix4x4> s_matrix_stack;
00038 
00039 void vrmlColor3f(float r, float g, float b)
00040 {
00041   vrmlColor4f(r, g, b, 1.f);
00042 }
00043 
00044 void vrmlColor4f(float r, float g, float b, float alpha)
00045 {
00046   g_vrml_color.r = r;
00047   g_vrml_color.g = g;
00048   g_vrml_color.b = b;
00049   g_vrml_color.alpha = alpha;
00050 }
00051 
00052 void vrmlRotatef(float phi, float x, float y, float z)
00053 {
00054   vrml::Matrix4x4 rot = vrml::Matrix4x4::rotate(DEG2RAD(phi), x, y, z);
00055   g_vrml_modelview = g_vrml_modelview * rot;
00056 }
00057 
00058 void vrmlTranslatef(float dx, float dy, float dz)
00059 {
00060   vrml::Matrix4x4 trans = vrml::Matrix4x4::translate(dx, dy, dz);
00061   g_vrml_modelview = g_vrml_modelview * trans;
00062 }
00063 
00064 void vrmlMultMatrix(const vrml::Matrix4x4& m)
00065 {
00066   g_vrml_modelview = g_vrml_modelview * m;
00067 }
00068 
00069 void vrmlScalef(float sx, float sy, float sz)
00070 {
00071   std::cerr << "WARNING " << __PRETTY_FUNCTION__ << " not working right now" << std::endl;
00072   vrml::Matrix4x4 scale = vrml::Matrix4x4::scale(sx, sy, sz);
00073   g_vrml_modelview = g_vrml_modelview * scale;
00074 }
00075 
00076 void vrmlPushMatrix()
00077 {
00078   s_matrix_stack.push(g_vrml_modelview);
00079 }
00080 
00081 void vrmlPopMatrix()
00082 {
00083   if (s_matrix_stack.size() == 0) {
00084     throw runtime_error("called vrmlPopMatrix() on an empty matrix stack");
00085   } else {
00086     g_vrml_modelview = s_matrix_stack.top();
00087     s_matrix_stack.pop();
00088   }
00089 }
00090 
00091 void vrmlLoadIdentity()
00092 {
00093   g_vrml_modelview = vrml::Matrix4x4::identity();
00094 }
00095 
00096 // helper functions
00097 void writeCurrentHead(std::ostream& os, bool allColor)
00098 {
00099   vrml::Vector4 origin(0, 0, 0, 1);
00100   vrml::Vector4 trans = g_vrml_modelview * origin;
00101   double dx = trans[0] / trans[3];
00102   double dy = trans[1] / trans[3];
00103   double dz = trans[2] / trans[3];
00104 
00105   // TODO handle scale inside the matrix?
00106   double rot[3][3];
00107   for (int i = 0; i < 3; ++i)
00108     for (int j = 0; j < 3; ++j)
00109       rot[i][j] = g_vrml_modelview[i][j];
00110   qglviewer::Quaternion quat;
00111   quat.setFromRotationMatrix(rot);
00112   qglviewer::Vec axis;
00113   float angle;
00114   quat.getAxisAngle(axis, angle);
00115 
00116   os
00117     << "Transform {\n"
00118     << "  rotation " << axis[0] << " " << axis[1] << " " << axis[2] << " " << angle << "\n"
00119     << "  translation " << dx << " " << dy << " " << dz << "\n"
00120     << "  children [\n"
00121     << "  Shape {\n"
00122     << "    appearance Appearance {\n"
00123     << "      material Material {\n"
00124     << "        diffuseColor " << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
00125     << "        ambientIntensity 0.2\n";
00126   if (allColor) {
00127     os
00128       << "        emissiveColor " << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
00129       << "        specularColor " << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n";
00130   } else {
00131     os
00132       << "        emissiveColor 0 0 0\n"
00133       << "        specularColor 0 0 0\n";
00134   }
00135   os
00136     << "        shininess 0.2\n"
00137     << "        transparency " << 1.0 - g_vrml_color.alpha << "\n"
00138     << "      }\n"
00139     << "    }\n"
00140     << "    geometry ";
00141 }
00142 
00143 void writeCurrentFoot(std::ostream& os)
00144 {
00145   os
00146     << "  }\n"
00147     << "  ]\n"
00148     << "}\n";
00149 }
00150 
00151 void writeScaleHead(std::ostream& os, double sx, double sy, double sz)
00152 {
00153   os
00154     << "Transform {\n"
00155     << "  scale " << sx << " " << sy << " " << sz << "\n"
00156     << "  children [\n"
00157     << "  Shape {\n"
00158     << "    appearance Appearance {\n"
00159     << "      material Material {\n"
00160     << "        diffuseColor " << g_vrml_color.r << " " << g_vrml_color.g << " " << g_vrml_color.b << "\n"
00161     << "        ambientIntensity 0.2\n"
00162     << "        emissiveColor 0 0 0\n"
00163     << "        specularColor 0 0 0\n"
00164     << "        shininess 0.2\n"
00165     << "        transparency " << 1.0 - g_vrml_color.alpha << "\n"
00166     << "      }\n"
00167     << "    }\n"
00168     << "    geometry ";
00169 }
00170 
00171 void writeScaleFoot(std::ostream& os)
00172 {
00173   os
00174     << "  }\n"
00175     << "  ]\n"
00176     << "}\n";
00177 }
00178 
00179 void writeBox(std::ostream& os, double l, double w, double h)
00180 {
00181   writeCurrentHead(os);
00182   os
00183     << "Box {\n"
00184     << "        size " << l << " " << w << " " << h << "\n"
00185     << "     }\n";
00186   writeCurrentFoot(os);
00187 }
00188 
00189 void writeSphere(std::ostream& os, double radius)
00190 {
00191   writeCurrentHead(os);
00192   os
00193     << "Sphere {\n"
00194     << "        radius " << radius << "\n"
00195     << "     }\n";
00196   writeCurrentFoot(os);
00197 }
00198 
00199 void writeEllipsoid(std::ostream& os, double r1, double r2, double r3)
00200 {
00201   writeCurrentHead(os);
00202   writeScaleHead(os, r1, r2, r3);
00203   os
00204     << "Sphere {\n"
00205     << "        radius 1.0\n"
00206     << "     }\n";
00207   writeScaleFoot(os);
00208   writeCurrentFoot(os);
00209 }
00210 
00211 void writeCone(std::ostream& os, double radius, double height)
00212 {
00213   writeCurrentHead(os);
00214   os
00215     << "Cone {\n"
00216     << "        bottomRadius " << radius << "\n"
00217     << "        height " << height << "\n"
00218     << "     }\n";
00219   writeCurrentFoot(os);
00220 }
00221 
00222 void writeCylinder(std::ostream& os, double radius, double height)
00223 {
00224   writeCurrentHead(os);
00225   os
00226     << "Cylinder {\n"
00227     << "        radius " << radius << "\n"
00228     << "        height " << height << "\n"
00229     << "     }\n";
00230   writeCurrentFoot(os);
00231 }
00232 
00233 void writePyramid(std::ostream& os, double length, double height)
00234 {
00235   (void) length; (void) height; // avoid warnings
00236   // TODO maybe use an indexedFaceSet ???
00237   os << "# writePyramid not implemented atm\n";
00238 }
00239 
00240 void writeVrmlFileHeader(std::ostream& os)
00241 {
00242   os << "#VRML V2.0 utf8\n";
00243 }
00244 
00245 void writePlane(std::ostream& os, double l, double w)
00246 {
00247   list<vrml_point_t> plane; // all the points
00248   plane.push_back(vrml_point_t(-l*0.5,  w*0.5, 0.));
00249   plane.push_back(vrml_point_t( l*0.5,  w*0.5, 0.));
00250   plane.push_back(vrml_point_t( l*0.5, -w*0.5, 0.));
00251   plane.push_back(vrml_point_t(-l*0.5, -w*0.5, 0.));
00252   writeQuads(os, plane.begin(), plane.end());
00253 }
00254 
00255 void writeSlice(std::ostream& os, double radius, double height, double fov, int slices_per_circle)
00256 {
00257   double fov_rad = fov/180.*M_PI; // convert to rad
00258   int num_slices = int(slices_per_circle * (fov_rad / (2*M_PI))) + 1;
00259   double angle_step = fov_rad / num_slices;
00260 
00261   double height_half = height * 0.5f;
00262   double lower_z = -height_half;
00263   double upper_z =  height_half;
00264 
00265   double last_x = std::cos(-fov_rad * 0.5) * radius;
00266   double last_y = std::sin(-fov_rad * 0.5) * radius;
00267 
00268 
00269   vrmlPushMatrix();
00270   list<vrml_point_t> triangles; // all the points
00271   triangles.push_back(vrml_point_t(0.f, 0.f, upper_z));
00272   triangles.push_back(vrml_point_t(0.f, 0.f, lower_z));
00273   triangles.push_back(vrml_point_t(last_x, last_y, upper_z));
00274   triangles.push_back(vrml_point_t(last_x, last_y, upper_z));
00275   triangles.push_back(vrml_point_t(last_x, last_y, lower_z));
00276   triangles.push_back(vrml_point_t(0.f, 0.f, lower_z));
00277 
00278   double start_angle = -0.5*fov_rad + angle_step;
00279   double angle       = start_angle;
00280   for (int i = 0; i < num_slices; ++i) {
00281     double x = std::cos(angle) * radius;
00282     double y = std::sin(angle) * radius;
00283 
00284     // lower triangle
00285     triangles.push_back(vrml_point_t(0.f, 0.f, lower_z));
00286     triangles.push_back(vrml_point_t(x, y, lower_z));
00287     triangles.push_back(vrml_point_t(last_x, last_y, lower_z));
00288     // upper
00289     triangles.push_back(vrml_point_t(0.f, 0.f, upper_z));
00290     triangles.push_back(vrml_point_t(x, y, upper_z));
00291     triangles.push_back(vrml_point_t(last_x, last_y, upper_z));
00292     //front rectangle (we use two triangles)
00293     triangles.push_back(vrml_point_t(last_x, last_y, upper_z));
00294     triangles.push_back(vrml_point_t(last_x, last_y, lower_z));
00295     triangles.push_back(vrml_point_t(x, y, upper_z));
00296     triangles.push_back(vrml_point_t(x, y, upper_z));
00297     triangles.push_back(vrml_point_t(x, y, lower_z));
00298     triangles.push_back(vrml_point_t(last_x, last_y, lower_z));
00299 
00300     last_x = x;
00301     last_y = y;
00302     angle += angle_step;
00303   }
00304 
00305   triangles.push_back(vrml_point_t(0.f, 0.f, upper_z));
00306   triangles.push_back(vrml_point_t(0.f, 0.f, lower_z));
00307   triangles.push_back(vrml_point_t(last_x, last_y, upper_z));
00308   triangles.push_back(vrml_point_t(last_x, last_y, upper_z));
00309   triangles.push_back(vrml_point_t(last_x, last_y, lower_z));
00310   triangles.push_back(vrml_point_t(0.f, 0.f, lower_z));
00311 
00312   // now we can write all the triangles
00313   writeTriangles(os, triangles.begin(), triangles.end());
00314   vrmlPopMatrix();
00315 }
00316 
00317 void writeAxes(std::ostream&os, double len)
00318 {
00319   writeCurrentHead(os);
00320   os << "#coordinate axes " << __func__ << std::endl;
00321   os << Axes(len) << endl;
00322   writeCurrentFoot(os);
00323 }
00324 
00325 void vrmlHeadTranslation(qglviewer::Vec& p )
00326 {
00327   vrml::Vector4 origin(0, 0, 0, 1);
00328   vrml::Vector4 trans = g_vrml_modelview * origin;
00329   p[0] = trans[0] / trans[3];
00330   p[1] = trans[1] / trans[3];
00331   p[2] = trans[2] / trans[3];
00332 }
00333 
00334 void vrmlHeadRotation(qglviewer::Quaternion& quat )
00335 {
00336   double rot[3][3];
00337   for (int i = 0; i < 3; ++i)
00338     for (int j = 0; j < 3; ++j)
00339       rot[i][j] = g_vrml_modelview[i][j];
00340 
00341   quat.setFromRotationMatrix(rot);
00342 }
00343 
00344 std::ostream& operator<<(std::ostream& os, const vrml_point_t& p)
00345 {
00346   os << p[0] << " " << p[1] << " " << p[2];
00347   return os;
00348 }
00349 
00350 void writePoseBox(std::ostream& os)
00351 {
00352   vrmlPushMatrix();
00353   vrmlPushMatrix();
00354   vrmlTranslatef(-0.5*0.5,0.5*0.25,0);
00355   vrmlColor3f(1.0, 0.3, 0.3);
00356   writeBox(os, 1*0.5, 0.25, 0.5);
00357   vrmlPopMatrix();
00358 
00359   vrmlPushMatrix();
00360   vrmlTranslatef(-0.5*0.5,-0.5*0.25,0);
00361   vrmlColor3f(1.0, 0.1, 0.1);
00362   writeBox(os, 1*0.5, 0.25, 0.5);
00363   vrmlPopMatrix();
00364 
00365   vrmlPushMatrix();
00366   vrmlTranslatef(0.5*0.5,0.5*0.25,0);
00367   vrmlColor3f(0.3, 0.3, 1.0);
00368   writeBox(os, 1*0.5, 0.25, 0.5);
00369   vrmlPopMatrix();
00370 
00371   vrmlPushMatrix();
00372   vrmlTranslatef(+0.5*0.5,-0.5*0.25,0);
00373   vrmlColor3f(0.1, 0.1, 1.);
00374   writeBox(os, 1*0.5, 0.25, 0.5);
00375   vrmlPopMatrix();
00376   vrmlPopMatrix();
00377 }
00378 
00379 void writeStart(std::ostream& os)
00380 {
00381   os << "Transform {\n"
00382     << "  translation 0 0 0" << std::endl
00383     << "  children [" << std::endl;
00384 }
00385 
00386 void writeEnd(std::ostream& os)
00387 {
00388   os << "  ]" << std::endl
00389     << "}" << std::endl;
00390 }


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:59