GLmodel.cpp
Go to the documentation of this file.
1 #include <cstdio>
2 #include <fstream>
3 #include <GL/glfw.h>
4 #include <sys/time.h>
5 #include "GLmodel.h"
6 
7 using namespace OpenHRP;
8 using namespace hrp;
9 
10 #define DEFAULT_W 640
11 #define DEFAULT_H 480
12 
13 GLcamera::GLcamera(const SensorInfo &i_si, GLlink *i_link) : m_name(i_si.name), m_link(i_link) {
14 
15  Matrix33 R;
16  Vector3 axis;
17  axis[0] = i_si.rotation[0];
18  axis[1] = i_si.rotation[1];
19  axis[2] = i_si.rotation[2];
20 
21  hrp::calcRodrigues(R, axis, i_si.rotation[3]);
22 
23  m_trans[ 0]=R(0,0);m_trans[ 1]=R(1,0);m_trans[ 2]=R(2,0);m_trans[3]=0;
24  m_trans[ 4]=R(0,1);m_trans[ 5]=R(1,1);m_trans[ 6]=R(2,1);m_trans[7]=0;
25  m_trans[ 8]=R(0,2);m_trans[ 9]=R(1,2);m_trans[10]=R(2,2);m_trans[11]=0;
26  m_trans[12]=i_si.translation[0];m_trans[13]=i_si.translation[1];
27  m_trans[14]=i_si.translation[2];m_trans[15]=1;
28 
29  m_near = i_si.specValues[0];
30  m_far = i_si.specValues[1];
31  m_fovy = i_si.specValues[2];
32  m_width = i_si.specValues[4];
33  m_height = i_si.specValues[5];
34 }
35 
36 GLcamera::GLcamera(int i_width, int i_height, double i_near, double i_far, double i_fovy) : m_near(i_near), m_far(i_far), m_fovy(i_fovy), m_width(i_width), m_height(i_height)
37 {
38 }
39 
40 const std::string& GLcamera::name() const {
41  return m_name;
42 }
43 
44 void GLcamera::computeAbsTransform(double o_trans[16]){
45  if (m_link){
46  double trans[16];
48  mulTrans(m_trans, trans, o_trans);
49  }else{
50  memcpy(o_trans, m_trans, sizeof(double)*16);
51  }
52 }
53 
54 void GLcamera::setView()
55 {
57 
58  glMatrixMode(GL_PROJECTION);
59  glLoadIdentity();
60  gluPerspective(fovy()*180/M_PI,
61  (double)width() / (double)height(),
62  near(), far());
63  gluLookAt(m_absTrans[12], m_absTrans[13], m_absTrans[14],
64  m_absTrans[12]-m_absTrans[8],
65  m_absTrans[13]-m_absTrans[9],
66  m_absTrans[14]-m_absTrans[10],
67  m_absTrans[4], m_absTrans[5], m_absTrans[6]);
68 }
69 
70 void GLcamera::setTransform(double i_trans[16]){
71  memcpy(m_trans, i_trans, sizeof(double)*16);
72 }
73 
74 void GLcamera::getAbsTransform(double o_trans[16]){
75  memcpy(o_trans, m_absTrans, sizeof(double)*16);
76 }
77 
78 void GLcamera::getDepthOfLine(int i_row, float *o_depth)
79 {
80  glReadPixels(0, i_row, width(), 1, GL_DEPTH_COMPONENT, GL_FLOAT, o_depth);
81 }
82 
83 
84 GLlink::GLlink(const LinkInfo &i_li, BodyInfo_var i_binfo) : m_parent(NULL), m_jointId(i_li.jointId){
85  Vector3 axis;
86  Matrix33 R;
87 
88  for (int i=0; i<3; i++){
89  m_axis[i] = i_li.jointAxis[i];
90  axis[i] = i_li.rotation[i];
91  }
92  setQ(0);
93 
94  hrp::calcRodrigues(R, axis, i_li.rotation[3]);
95 
96  m_trans[ 0]=R(0,0);m_trans[ 1]=R(1,0);m_trans[ 2]=R(2,0);m_trans[3]=0;
97  m_trans[ 4]=R(0,1);m_trans[ 5]=R(1,1);m_trans[ 6]=R(2,1);m_trans[7]=0;
98  m_trans[ 8]=R(0,2);m_trans[ 9]=R(1,2);m_trans[10]=R(2,2);m_trans[11]=0;
99  m_trans[12]=i_li.translation[0];m_trans[13]=i_li.translation[1];
100  m_trans[14]=i_li.translation[2];m_trans[15]=1;
101 
102 
103  m_list = glGenLists(1);
104  //std::cout << i_li.name << std::endl;
105  //printMatrix(m_trans);
106 
107  glNewList(m_list, GL_COMPILE);
108 
109  ShapeInfoSequence_var sis = i_binfo->shapes();
110  AppearanceInfoSequence_var ais = i_binfo->appearances();
111  MaterialInfoSequence_var mis = i_binfo->materials();
112  const TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
113  for (unsigned int l=0; l<tsis.length(); l++){
114  const TransformedShapeIndex &tsi = tsis[l];
115  double tform[16];
116  for (int i=0; i<3; i++){
117  for (int j=0; j<4; j++){
118  tform[j*4+i] = tsi.transformMatrix[i*4+j];
119  }
120  }
121  tform[3] = tform[7] = tform[11] = 0.0; tform[15] = 1.0;
122 
123  glPushMatrix();
124  glMultMatrixd(tform);
125  //printMatrix(tform);
126 
127  glBegin(GL_TRIANGLES);
128  short index = tsi.shapeIndex;
129  ShapeInfo& si = sis[index];
130  const float *vertices = si.vertices.get_buffer();
131  const LongSequence& triangles = si.triangles;
132  const AppearanceInfo& ai = ais[si.appearanceIndex];
133  const float *normals = ai.normals.get_buffer();
134  //std::cout << "length of normals = " << ai.normals.length() << std::endl;
135  const LongSequence& normalIndices = ai.normalIndices;
136  //std::cout << "length of normalIndices = " << normalIndices.length() << std::endl;
137  const int numTriangles = triangles.length() / 3;
138  //std::cout << "numTriangles = " << numTriangles << std::endl;
139  if (ai.materialIndex >= 0){
140  const MaterialInfo& mi = mis[ai.materialIndex];
141  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE,
142  mi.diffuseColor);
143  }else{
144  std::cout << "no material" << std::endl;
145  }
146  for(int j=0; j < numTriangles; ++j){
147  if (!ai.normalPerVertex){
148  int p;
149  if (normalIndices.length() == 0){
150  p = j*3;
151  }else{
152  p = normalIndices[j]*3;
153  }
154  glNormal3fv(normals+p);
155  }
156  for(int k=0; k < 3; ++k){
157  if (ai.normalPerVertex){
158  int p = normalIndices[j*3+k]*3;
159  glNormal3fv(normals+p);
160  }
161  long orgVertexIndex = si.triangles[j * 3 + k];
162  int p = orgVertexIndex * 3;
163  glVertex3fv(vertices+p);
164  }
165  }
166  glEnd();
167  glPopMatrix();
168  }
169 
170  glEndList();
171 
172  const SensorInfoSequence& sensors = i_li.sensors;
173  for (unsigned int i=0; i<sensors.length(); i++){
174  const SensorInfo& si = sensors[i];
175  std::string type(si.type);
176  if (type == "Vision"){
177  //std::cout << si.name << std::endl;
178  m_cameras.push_back(new GLcamera(si, this));
179  }
180  }
181 
182 }
183 
184 void GLlink::draw(){
185  glPushMatrix();
186  glMultMatrixd(m_trans);
187  glMultMatrixd(m_T_j);
188  glCallList(m_list);
189  for (unsigned int i=0; i<m_children.size(); i++){
190  m_children[i]->draw();
191  }
192  glPopMatrix();
193 }
194 
195 void GLlink::setParent(GLlink *i_parent){
196  m_parent = i_parent;
197 }
198 
199 void GLlink::addChild(GLlink *i_child){
200  i_child->setParent(this);
201  m_children.push_back(i_child);
202 }
203 
204 void GLlink::setQ(double i_q){
205  Matrix33 R;
206  hrp::calcRodrigues(R, m_axis, i_q);
207  m_T_j[ 0]=R(0,0);m_T_j[ 1]=R(1,0);m_T_j[ 2]=R(2,0);m_T_j[3]=0;
208  m_T_j[ 4]=R(0,1);m_T_j[ 5]=R(1,1);m_T_j[ 6]=R(2,1);m_T_j[7]=0;
209  m_T_j[ 8]=R(0,2);m_T_j[ 9]=R(1,2);m_T_j[10]=R(2,2);m_T_j[11]=0;
210  m_T_j[12]=0; m_T_j[13]=0; m_T_j[14]=0; m_T_j[15]=1;
211  //printf("m_T_j:\n");
212  //printMatrix(m_T_j);
213 }
214 
215 void GLlink::setTransform(double i_trans[16]){
216  memcpy(m_trans, i_trans, sizeof(double)*16);
217 }
218 
220  return m_jointId;
221 }
222 
223 GLcamera *GLlink::findCamera(const char *i_name){
224  std::string name(i_name);
225  for (unsigned int i=0; i<m_cameras.size(); i++){
226  if (m_cameras[i]->name() == name) return m_cameras[i];
227  }
228  return NULL;
229 }
230 
231 void GLlink::computeAbsTransform(double o_trans[16]){
232  if (m_parent){
233  double trans1[16], trans2[16];
234  mulTrans(m_T_j, m_trans, trans1);
235  m_parent->computeAbsTransform(trans2);
236  mulTrans(trans1, trans2, o_trans);
237  }else{
238  memcpy(o_trans, m_trans, sizeof(double)*16);
239  }
240 }
241 
242 GLbody::GLbody(BodyInfo_var i_binfo){
243  LinkInfoSequence_var lis = i_binfo->links();
244 
245  for (unsigned int i=0; i<lis->length(); i++){
246  m_links.push_back(new GLlink(lis[i], i_binfo));
247  }
248  // setup tree
249  for (unsigned int i=0; i<m_links.size(); i++){
250  const LinkInfo &li = lis[i];
251  if (li.parentIndex < 0) m_root = m_links[i];
252  for (unsigned int j=0; j<li.childIndices.length(); j++){
253  m_links[i]->addChild(m_links[li.childIndices[j]]);
254  }
255  }
256 
257 }
258 
260  for (unsigned int i=0; i<m_links.size(); i++){
261  delete m_links[i];
262  }
263 }
264 
265 void GLbody::setPosture(double *i_angles, double *i_pos, double *i_rpy){
266  double tform[16];
267  Matrix33 R = rotFromRpy(i_rpy[0], i_rpy[1], i_rpy[2]);
268  tform[ 0]=R(0,0);tform[ 1]=R(1,0);tform[ 2]=R(2,0);tform[ 3]=0;
269  tform[ 4]=R(0,1);tform[ 5]=R(1,1);tform[ 6]=R(2,1);tform[ 7]=0;
270  tform[ 8]=R(0,2);tform[ 9]=R(1,2);tform[10]=R(2,2);tform[11]=0;
271  tform[12]=i_pos[0];tform[13]=i_pos[1];tform[14]=i_pos[2];tform[15]=1;
272  m_root->setTransform(tform);
273  for (unsigned int i=0; i<m_links.size(); i++){
274  int id = m_links[i]->jointId();
275  if (id >= 0){
276  m_links[i]->setQ(i_angles[id]);
277  }
278  }
279 }
280 
281 void GLbody::draw(){
282  m_root->draw();
283 }
284 
285 GLcamera *GLbody::findCamera(const char *i_name){
286  for (unsigned int i=0; i<m_links.size(); i++){
287  GLcamera *camera = m_links[i]->findCamera(i_name);
288  if (camera) return camera;
289  }
290  return NULL;
291 }
292 
293 void GLscene::addBody(GLbody *i_body){
294  m_bodies.push_back(i_body);
295 }
296 
297 void GLscene::draw(bool swap){
298  m_camera->setView();
299 #if 0
300  struct timeval tv1, tv2;
301  gettimeofday(&tv1, NULL);
302 #endif
303  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
304 
305  glMatrixMode(GL_MODELVIEW);
306 
307  for (unsigned int i=0; i<m_bodies.size(); i++){
308  m_bodies[i]->draw();
309  }
310 
311  if (swap) glfwSwapBuffers();
312 #if 0
313  gettimeofday(&tv2, NULL);
314  //std::cout << "t = " << ((tv2.tv_sec - tv1.tv_sec)*1e3 + (tv2.tv_usec - tv1.tv_usec)*1e-3) << "[ms]" << std::endl;
315  //std::cout << (int)(1.0/((tv2.tv_sec - tv1.tv_sec) + (tv2.tv_usec - tv1.tv_usec)*1e-6)) << "[FPS]" << std::endl;
316 #endif
317 }
318 
319 void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
320 {
321  for (int i=0; i<4; i++){
322  for (int j=0; j<4;j++){
323  double v = 0;
324  for (int k=0; k<4; k++){
325  v += i_m1[i*4+k]*i_m2[j+k*4];
326  }
327  o_m[i*4+j] = v;
328  }
329  }
330 }
331 
333 {
334  return m_scene;
335 }
336 
337 void printMatrix(double mat[16])
338 {
339  for (int i=0; i<4; i++){
340  for (int j=0; j<4; j++){
341  printf("%6.3f ", mat[j*4+i]);
342  }
343  printf("\n");
344  }
345 }
346 
347 GLscene::GLscene() : m_camera(m_default_camera)
348 {
349  m_default_camera = new GLcamera(DEFAULT_W, DEFAULT_H, 1.0, 100.0, 40*M_PI/180);
350  double T[] = {0,1,0,0,
351  0,0,1,0,
352  1,0,0,0,
353  4,0,0.8,1};
355 }
356 
358 {
359  delete m_default_camera;
360 }
361 
363 {
364  glfwInit();
365 
366  glfwOpenWindow(DEFAULT_W,DEFAULT_H,0,0,0,0,24,0, GLFW_WINDOW);
367 
369 
370  GLfloat light0pos[] = { 0.0, 4.0, 6.0, 1.0 };
371  GLfloat light1pos[] = { 6.0, 4.0, 0.0, 1.0 };
372  GLfloat white[] = { 0.6, 0.6, 0.6, 1.0 };
373 
374  glClearColor(0, 0, 0, 1.0);
375  glEnable(GL_DEPTH_TEST);
376 
377  glEnable(GL_CULL_FACE);
378  glCullFace(GL_BACK);
379 
380  glEnable(GL_LIGHTING);
381  glEnable(GL_LIGHT0);
382  glEnable(GL_LIGHT1);
383  glLightfv(GL_LIGHT0, GL_DIFFUSE, white);
384  glLightfv(GL_LIGHT0, GL_SPECULAR, white);
385  glLightfv(GL_LIGHT1, GL_DIFFUSE, white);
386  glLightfv(GL_LIGHT1, GL_SPECULAR, white);
387  glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
388  glLightfv(GL_LIGHT1, GL_POSITION, light1pos);
389 }
390 
391 void GLscene::save(const char *i_fname)
392 {
393  int w, h;
394  glfwGetWindowSize(&w,&h);
395  unsigned char *buffer = new unsigned char[w*h*3];
396 
397  capture(buffer);
398  std::ofstream ofs(i_fname, std::ios::out | std::ios::trunc | std::ios::binary );
399  char buf[10];
400  sprintf(buf, "%d %d", w, h);
401  ofs << "P6" << std::endl << buf << std::endl << "255" << std::endl;
402  for (int i=0; i<h; i++){
403  ofs.write((char *)(buffer+i*w*3), w*3);
404  }
405  delete [] buffer;
406 }
407 
408 void GLscene::capture(unsigned char *o_buffer)
409 {
410  int w, h;
411  glfwGetWindowSize(&w,&h);
412  glReadBuffer(GL_BACK);
413  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
414  for (int i=0; i<h; i++){
415  glReadPixels(0,(h-1-i),w,1,GL_RGB,GL_UNSIGNED_BYTE,
416  o_buffer + i*3*w);
417  }
418 }
419 
421 {
422  if (!i_camera) return;
423 
424  m_camera = i_camera;
425  glfwSetWindowSize(m_camera->width(), m_camera->height());
426  glViewport(0, 0, m_camera->width(), m_camera->height());
427 }
428 
430 {
431  return m_camera;
432 }
433 
435 
436 
437 unsigned int GLscene::numBodies() const
438 {
439  return m_bodies.size();
440 }
441 
442 GLbody *GLscene::body(unsigned int i_rank)
443 {
444  if (i_rank >= numBodies()) return NULL;
445  return m_bodies[i_rank];
446 }
size_t draw()
Definition: GLbody.cpp:59
std::vector< GLbody * > m_bodies
Definition: GLmodel.h:97
unsigned int height()
Definition: GLcamera.h:31
GLscene()
Definition: GLmodel.cpp:347
double * getAbsTransform()
Definition: GLcamera.cpp:108
void init()
Definition: GLmodel.cpp:362
void save(const char *i_fname)
Definition: GLmodel.cpp:391
png_infop png_charp png_int_32 png_int_32 int * type
double m_trans[16]
Definition: GLmodel.h:25
unsigned int numBodies() const
Definition: GLmodel.cpp:437
double near()
Definition: GLcamera.h:27
void addBody(GLbody *i_body)
Definition: GLmodel.cpp:293
~GLbody()
Definition: GLbody.cpp:18
void white()
png_infop png_charpp name
void setView()
Definition: GLcamera.cpp:73
void printMatrix(double mat[16])
Definition: GLmodel.cpp:337
const std::string & name() const
Definition: GLcamera.cpp:55
w
OpenHRP::matrix33 Matrix33
png_uint_32 i
double fovy()
Definition: GLcamera.h:29
GLlink * m_link
Definition: GLcamera.h:47
png_infop png_bytep * trans
GLcamera * m_camera
Definition: GLmodel.h:98
Definition: GLbody.h:11
GLcamera * getCamera()
Definition: GLmodel.cpp:429
Matrix33 rotFromRpy(const Vector3 &rpy)
static GLscene * getInstance()
Definition: GLmodel.cpp:332
GLbody * body(unsigned int i_rank)
Definition: GLmodel.cpp:442
OpenHRP::vector3 Vector3
#define DEFAULT_H
Definition: GLmodel.cpp:11
int gettimeofday(struct timeval *tv, struct timezone *tz)
static GLscene * m_scene
Definition: GLmodel.h:96
unsigned int width()
Definition: GLcamera.h:30
GLcamera * m_default_camera
Definition: GLmodel.h:98
list index
HRP_UTIL_EXPORT void calcRodrigues(Matrix44 &out_R, const Vector3 &axis, double q)
GLcamera * findCamera(const char *i_name)
Definition: GLbody.cpp:71
void setTransform(double i_trans[16])
Definition: GLmodel.cpp:70
png_bytep buf
GLcamera(int i_width, int i_height, double i_near, double i_far, double i_fovy, GLlink *i_link=NULL, int i_id=-1)
Definition: GLcamera.cpp:21
void getDepthOfLine(int i_row, float *o_depth)
Definition: GLmodel.cpp:78
double m_absTrans[16]
Definition: GLcamera.h:46
#define M_PI
void draw()
Definition: IrrModel.cpp:496
void setPosture(const double *i_angles)
Definition: GLbody.cpp:21
std::string sprintf(char const *__restrict fmt,...)
void setCamera(GLcamera *i_camera)
Definition: GLmodel.cpp:420
void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
Definition: GLmodel.cpp:319
png_infop png_bytep buffer
void computeAbsTransform(double o_trans[16])
Definition: GLcamera.cpp:63
void capture()
Definition: GLsceneBase.h:67
~GLscene()
Definition: GLmodel.cpp:357
std::string m_name
Definition: GLcamera.h:45
GLbody()
Definition: GLbody.cpp:14
#define DEFAULT_W
Definition: GLmodel.cpp:10
double far()
Definition: GLcamera.h:28


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20