00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
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
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;
00236
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;
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;
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;
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
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
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
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
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 }
