IrrModel.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include "IrrModel.h"
3 
4 using namespace irr;
5 
6 using namespace core;
7 using namespace scene;
8 using namespace video;
9 using namespace io;
10 
11 using namespace OpenHRP;
12 using namespace hrp;
13 
14 class MyEventReceiver : public IEventReceiver
15 {
16 public:
17  MyEventReceiver(ICameraSceneNode *i_camera, double i_radius=3.0) :
18  m_camera(i_camera),
19  m_radius(i_radius),
20  m_pan(-M_PI/6),
21  m_tilt(0){
22  updateEye();
23  }
24 
25  virtual bool OnEvent(const SEvent& event)
26  {
27  if(event.EventType == EET_KEY_INPUT_EVENT)
28  {
29  if(event.KeyInput.PressedDown)
30  {
31  switch(event.KeyInput.Key)
32  {
33  case KEY_LEFT:
34  return true;
35  case KEY_RIGHT:
36  return true;
37  case KEY_UP:
38  return true;
39  case KEY_DOWN:
40  return true;
41  default:
42  return false;
43  }
44  }
45  return true;
46  }
47  if(event.EventType == EET_MOUSE_INPUT_EVENT)
48  {
49  switch(event.MouseInput.Event)
50  {
51  case EMIE_LMOUSE_PRESSED_DOWN:
52  m_mouse.X = event.MouseInput.X;
53  m_mouse.Y = event.MouseInput.Y;
54  return true;
55  case EMIE_MOUSE_WHEEL:
56  //上回転
57  if(event.MouseInput.Wheel == 1)
58  {
59  if (m_radius > 0.001){
60  m_radius *= 0.9;
61  updateEye();
62  }
63  }
64  //下回転
65  else if(event.MouseInput.Wheel == -1)
66  {
67  m_radius *= 1.1;
68  updateEye();
69  }
70  return true;
71  case EMIE_MOUSE_MOVED:
72  if (event.MouseInput.isLeftPressed()){
73  s32 dx = event.MouseInput.X - m_mouse.X;
74  s32 dy = event.MouseInput.Y - m_mouse.Y;
75  m_pan += dx*0.01;
76  m_tilt += dy*0.01;
77  if (m_tilt < -M_PI/2) m_tilt = -M_PI/2;
78  if (m_tilt > M_PI/2) m_tilt = M_PI/2;
79  updateEye();
80  m_mouse.X = event.MouseInput.X;
81  m_mouse.Y = event.MouseInput.Y;
82  }
83  return true;
84 
85  default:
86  return true;
87  }
88  }
89  return false;
90  }
91 private:
92  void updateEye(){
93  vector3df target = m_camera->getTarget();
94  m_eye.X = target.X + m_radius*cos(m_tilt)*cos(m_pan);
95  m_eye.Y = target.Y + m_radius*cos(m_tilt)*sin(m_pan);
96  m_eye.Z = target.Z + m_radius*sin(m_tilt);
97  m_camera->setPosition(m_eye);
98  }
99  ICameraSceneNode *m_camera;
100  position2d<s32> m_mouse;
101  vector3df m_eye;
102  float m_radius, m_pan, m_tilt;
103 };
104 
105 class GLlink : public ISceneNode
106 {
107 public:
108  GLlink(ISceneNode *i_parent, ISceneManager *i_mgr, s32 i_id,
109  const LinkInfo &i_li, BodyInfo_var i_binfo) :
110  ISceneNode(i_parent, i_mgr, i_id),
111  m_jointId(i_li.jointId){
112  setAutomaticCulling(scene::EAC_OFF);
113 
114 
115  setPosition(vector3df( i_li.translation[0],
116  -i_li.translation[1],
117  i_li.translation[2]));
118  Vector3 axis(i_li.rotation[0],
119  i_li.rotation[1],
120  i_li.rotation[2]);
121  Matrix33 R;
122  hrp::calcRodrigues(R, axis, i_li.rotation[3]);
123  Vector3 rpy(rpyFromRot(R));
124  //std::cout << "rpy:" << rpy << std::endl;
125  setRotation(vector3df(-180/M_PI*rpy[0],
126  180/M_PI*rpy[1],
127  -180/M_PI*rpy[2]));
128 
129  m_axis << i_li.jointAxis[0], i_li.jointAxis[1], i_li.jointAxis[2];
130 
131  ShapeInfoSequence_var sis = i_binfo->shapes();
132  AppearanceInfoSequence_var ais = i_binfo->appearances();
133  MaterialInfoSequence_var mis = i_binfo->materials();
134  TextureInfoSequence_var txs = i_binfo->textures();
135  const TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
136 
137 
138  core::vector3df vertex;
139  core::vector3df normal;
140 
141  for (unsigned int l=0; l<tsis.length(); l++){
142  SMesh* mesh = new SMesh();
143  SMeshBuffer* meshBuffer = new SMeshBuffer();
144  mesh->addMeshBuffer(meshBuffer);
145  meshBuffer->drop();
146 
147  const TransformedShapeIndex &tsi = tsis[l];
148  short index = tsi.shapeIndex;
149  ShapeInfo& si = sis[index];
150  const float *vertices = si.vertices.get_buffer();
151  const LongSequence& triangles = si.triangles;
152  const AppearanceInfo& ai = ais[si.appearanceIndex];
153  const float *normals = ai.normals.get_buffer();
154  //std::cout << "length of normals = " << ai.normals.length() << std::endl;
155  const LongSequence& normalIndices = ai.normalIndices;
156  //std::cout << "length of normalIndices = " << normalIndices.length() << std::endl;
157  const int numTriangles = triangles.length() / 3;
158  //std::cout << "numTriangles = " << numTriangles << std::endl;
159 
160  video::SColor color(0xffffffff);
161  if (ai.colors.length()){
162  color.set(0xff,
163  0xff*ai.colors[0],
164  0xff*ai.colors[1],
165  0xff*ai.colors[2]);
166  }else if (ai.materialIndex >= 0){
167  const MaterialInfo& mi = mis[ai.materialIndex];
168  color.set(0xff,
169  0xff*mi.diffuseColor[0],
170  0xff*mi.diffuseColor[1],
171  0xff*mi.diffuseColor[2]);
172  }else{
173  std::cout << "no material" << std::endl;
174  }
175 
176 
177  SMeshBuffer* mb = reinterpret_cast<SMeshBuffer*>(mesh->getMeshBuffer(mesh->getMeshBufferCount()-1));
178  u32 vCount = mb->getVertexCount();
179 
180  const DblArray12& tfm = tsi.transformMatrix;
181  CMatrix4<f32> cmat;
182  for (int i=0; i<3; i++){
183  for (int j=0; j<4; j++){
184  cmat[j*4+i] = tfm[i*4+j];
185  }
186  }
187  cmat[3] = cmat[7] = cmat[11] = 0.0; cmat[15] = 1.0;
188  vector3df pos = cmat.getTranslation();
189  pos.Y *= -1;
190  vector3df rpy = cmat.getRotationDegrees();
191  rpy.X *= -1;
192  rpy.Z *= -1;
193  vector3df scale = cmat.getScale();
194 
195  const float *textureCoordinate = NULL;
196  if (ai.textureIndex >= 0){
197  textureCoordinate = ai.textureCoordinate.get_buffer();
198  //std::cout << "length of textureCoordinate:" << ai.textureCoordinate.length() << std::endl;
199  //std::cout << "length of vertices:" << si.vertices.length() << std::endl;
200 
201  }
202 
203  for(int j=0; j < numTriangles; ++j){
204  if (!ai.normalPerVertex){
205  int p;
206  if (normalIndices.length() == 0){
207  p = j*3;
208  }else{
209  p = normalIndices[j]*3;
210  }
211  if ( normals != NULL ) {
212  normal.X = normals[p];
213  normal.Y = -normals[p+1]; //left-handed->right-handed
214  normal.Z = normals[p+2];
215  } else {
216  normal.X = 0;
217  normal.Y = 0;
218  normal.Z = 1;
219  }
220  }
221  for(int k=0; k < 3; ++k){
222  long orgVertexIndex = si.triangles[j * 3 + k];
223  if (ai.normalPerVertex){
224  int p;
225  if (normalIndices.length()){
226  p = normalIndices[j*3+k]*3;
227  }else{
228  p = orgVertexIndex*3;
229  }
230  normal.X = normals[p];
231  normal.Y = -normals[p+1]; //left-handed -> right-handed
232  normal.Z = normals[p+2];
233  }
234  int p = orgVertexIndex * 3;
235  vertex.X = scale.X*vertices[p];
236  vertex.Y = -scale.Y*vertices[p+1]; // left-handed -> right-handed
237  vertex.Z = scale.Z*vertices[p+2];
238  //std::cout << vertices[p] <<"," << vertices[p+1] << "," << vertices[p+2] << std::endl;
239  vector2df texc;
240  if (textureCoordinate){
241 
242  texc.X = textureCoordinate[ai.textureCoordIndices[j*3+k]*2];
243  texc.Y = textureCoordinate[ai.textureCoordIndices[j*3+k]*2+1];
244  }
245  // redundant vertices
246  mb->Vertices.push_back(video::S3DVertex(vertex,normal,color, texc));
247  }
248  mb->Indices.push_back(vCount);
249  mb->Indices.push_back(vCount+2);
250  mb->Indices.push_back(vCount+1);
251  vCount += 3;
252  }
253  mesh->getMeshBuffer(0)->recalculateBoundingBox();
254 
255  // Create the Animated mesh if there's anything in the mesh
256  SAnimatedMesh* pAM = 0;
257  if ( 0 != mesh->getMeshBufferCount() )
258  {
259  mesh->recalculateBoundingBox();
260  pAM = new SAnimatedMesh();
261  pAM->Type = EAMT_OBJ;
262  pAM->addMesh(mesh);
263  pAM->recalculateBoundingBox();
264  }
265 
266  mesh->drop();
267 
268  vector3df noscale(1,1,1);
269 
270  IMeshSceneNode *node
271  = i_mgr->addMeshSceneNode(mesh, this, -1,
272  pos,
273  rpy,
274  noscale);
275 
276  if (ai.textureIndex >= 0){
277  const TextureInfo& ti = txs[ai.textureIndex];
278  //std::cout << "url:" << ti.url << std::endl;
279  video::IVideoDriver* driver = i_mgr->getVideoDriver();
280  const char *path = ti.url;
281  SMaterial& mat = node->getMaterial(0);
282  ITexture *texture = driver->getTexture(path);
283  mat.setTexture( 0, texture);
284  }
285 
286  }
287 
288  const SensorInfoSequence& sensors = i_li.sensors;
289  for (unsigned int i=0; i<sensors.length(); i++){
290  const SensorInfo& si = sensors[i];
291  std::string type(si.type);
292  if (type == "Vision"){
293  //std::cout << si.name << std::endl;
294  ISceneNode *camera = i_mgr->addEmptySceneNode(this);
295  camera->setName(si.name);
296  camera->setPosition(vector3df( si.translation[0],
297  -si.translation[1],
298  si.translation[2]));
299  Vector3 axis(si.rotation[0],
300  si.rotation[1],
301  si.rotation[2]);
302  Matrix33 R;
303  hrp::calcRodrigues(R, axis, si.rotation[3]);
304  Vector3 rpy(rpyFromRot(R));
305  camera->setRotation(vector3df(-180/M_PI*rpy[0],
306  180/M_PI*rpy[1],
307  -180/M_PI*rpy[2]));
308  m_cameraInfos.push_back(new GLcamera(si, camera));
309  }
310  }
311  }
312  virtual void render() {}
313  virtual const aabbox3d<f32>& getBoundingBox() const { return m_box; }
314  void setQ(double i_q){
315  Matrix33 R;
316  hrp::calcRodrigues(R, m_axis, i_q);
317  Vector3 rpy(rpyFromRot(R));
318  rpy *= 180/M_PI;
319  vector3df euler(-rpy[0], rpy[1], -rpy[2]);
320  setRotation(euler);
321  }
322  int jointId() const { return m_jointId; }
323  GLcamera *findCamera(const char *i_name){
324  for (unsigned int i=0; i<m_cameraInfos.size(); i++){
325  if (strcmp(i_name, m_cameraInfos[i]->name())==0) return m_cameraInfos[i];
326  }
327  return NULL;
328  }
329 private:
330  aabbox3d<f32> m_box;
332  int m_jointId;
333  std::vector<GLcamera *> m_cameraInfos;
334 };
335 
336 GLbody::GLbody(ISceneNode *i_parent, ISceneManager *i_mgr, s32 i_id,
337  BodyInfo_var i_binfo) :
338  ISceneNode(i_parent, i_mgr, i_id){
339  setAutomaticCulling(scene::EAC_OFF);
340 
341  LinkInfoSequence_var lis = i_binfo->links();
342  //std::cout << "creating links" << std::endl;
343  for (unsigned int i=0; i<lis->length(); i++){
344  m_links.push_back(
345  new GLlink(i_mgr->getRootSceneNode(), i_mgr,
346  -1, lis[i], i_binfo));
347  }
348  //std::cout << "creating a tree" << std::endl;
349  // setup tree
350  for (unsigned int i=0; i<m_links.size(); i++){
351  const LinkInfo &li = lis[i];
352  if (li.parentIndex < 0) {
353  m_root = m_links[i];
354  addChild(m_links[i]);
355  }
356  for (unsigned int j=0; j<li.childIndices.length(); j++){
357  m_links[i]->addChild(m_links[li.childIndices[j]]);
358  }
359  }
360  //std::cout << "done" << std::endl;
361 }
362 
363 void GLbody::setPosture(double *i_angles, double *i_pos, double *i_rpy){
364  m_root->setPosition(vector3df(i_pos[0], -i_pos[1], i_pos[2]));
365  m_root->setRotation(vector3df(-180/M_PI*i_rpy[0],
366  180/M_PI*i_rpy[1],
367  -180/M_PI*i_rpy[2]));
368  for (unsigned int i=0; i<m_links.size(); i++){
369  int id = m_links[i]->jointId();
370  if (id >= 0){
371  m_links[i]->setQ(i_angles[id]);
372  }
373  }
374 }
375 
376 GLcamera *GLbody::findCamera(const char *i_name)
377 {
378  for (unsigned int i=0; i<m_links.size(); i++){
379  GLcamera *ci = m_links[i]->findCamera(i_name);
380  if (ci) return ci;
381  }
382  return NULL;
383 }
384 
385 GLcamera::GLcamera(const OpenHRP::SensorInfo& i_info, ISceneNode *i_node) :
386  m_node(i_node)
387 {
388  m_near = i_info.specValues[0];
389  m_far = i_info.specValues[1];
390  m_fovy = i_info.specValues[2];
391  m_width = i_info.specValues[4];
392  m_height = i_info.specValues[5];
393 }
394 
395 
396 void GLcamera::setCameraParameters(ICameraSceneNode *i_camera)
397 {
398 #if 0 // This doesn't work
399  i_camera->setAspectRatio(((float)m_width)/m_height);
400  i_camera->setNearValue(m_near);
401  i_camera->setFarValue(m_far);
402  i_camera->setFOV(m_fov);
403 #else
404  matrix4 m;
405  double t = m_near*tan(m_fovy/2), r = (t*m_width)/m_height;
406  m[0] = m_near/r;
407  m[1] = 0;
408  m[2] = 0;
409  m[3] = 0;
410 
411  m[4] = 0;
412  m[5] = m_near/t;
413  m[6] = 0;
414  m[7] = 0;
415 
416  m[8] = 0;
417  m[9] = 0;
418  m[10] = (m_far+m_near)/(m_far-m_near);
419  m[11] = 1;
420 
421  m[12] = 0;
422  m[13] = 0;
423  m[14] = -2*m_far*m_near/(m_far-m_near);
424  m[15] = 0;
425  i_camera->setProjectionMatrix(m);
426 #endif
427 }
428 
429 void updateAbsoluteTransformation(ISceneNode *i_node)
430 {
431  ISceneNode *parent = i_node->getParent();
432  if (parent){
434  }
435  i_node->updateAbsolutePosition();
436 }
437 
438 void GLcamera::updateCameraTransform(ICameraSceneNode *i_camera)
439 {
441  matrix4 mat = m_node->getAbsoluteTransformation();
442  vector3df pos = mat.getTranslation();
443  i_camera->setPosition(pos);
444  vector3df view(pos.X-mat[8],pos.Y-mat[9],pos.Z-mat[10]); // -Z
445  vector3df up(-mat[4], -mat[5], -mat[6]); // -Y axis
446  i_camera->setTarget(view);
447  i_camera->setUpVector(up);
448 }
449 
450 const char *GLcamera::name()
451 {
452  return m_node->getName();
453 }
454 
455 int GLcamera::width()
456 {
457  return m_width;
458 }
459 
460 int GLcamera::height()
461 {
462  return m_height;
463 }
464 
465 void GLcamera::getAbsTransform(double *o_T)
466 {
467  matrix4 mat = m_node->getAbsoluteTransformation();
468  vector3df pos = mat.getTranslation();
469  vector3df rpy = mat.getRotationDegrees();
470  Matrix33 R(rotFromRpy(-rpy.X*M_PI/180, rpy.Y*M_PI/180, -rpy.Z*M_PI/180));
471  o_T[ 0] = R(0,0);o_T[ 4] = R(0,1);o_T[ 8] = R(0,2);o_T[12] = pos.X;
472  o_T[ 1] = R(1,0);o_T[ 5] = R(1,1);o_T[ 9] = R(1,2);o_T[13] = -pos.Y;
473  o_T[ 2] = R(2,0);o_T[ 6] = R(2,1);o_T[10] = R(2,2);o_T[14] = pos.Z;
474  o_T[ 3] = 0; o_T[ 7] = 0; o_T[11] = 0; ;o_T[15] = 1.0;
475 }
476 
477 float GLcamera::near()
478 {
479  return m_near;
480 }
481 
482 float GLcamera::far()
483 {
484  return m_far;
485 }
486 
487 float GLcamera::fovy()
488 {
489  return m_fovy;
490 }
491 
492 GLscene::GLscene() : m_device(NULL), m_camera(NULL), m_cnode(NULL)
493 {
494 }
495 
497 {
498  m_device->run();
499  if (m_camera != m_defaultCamera) m_camera->updateCameraTransform(m_cnode);
500  m_device->getVideoDriver()->beginScene(true, true, SColor(255,100,101,140));
501  m_device->getSceneManager()->drawAll();
502  m_device->getVideoDriver()->endScene();
503 
504  int fps = m_device->getVideoDriver()->getFPS();
505  int prims = m_device->getVideoDriver()->getPrimitiveCountDrawn();
506  wchar_t tmp[1024];
507  swprintf(tmp, 1024, L"Irrlicht (fps:%d) Triangles:%d", fps, prims);
508  m_device->setWindowCaption(tmp);
509 }
510 
512 {
513  if (m_defaultCamera) delete m_defaultCamera;
514 }
515 
516 GLscene *GLscene::m_scene = NULL;
517 
519 {
520  if (!m_scene) m_scene = new GLscene;
521  return m_scene;
522 }
523 
524 bool GLscene::init(int w, int h)
525 {
526  m_device =
527  createDevice( video::EDT_OPENGL, dimension2d<u32>(w, h), 32,
528  false, false, false, 0);
529 
530  if (!m_device) return false;
531 
532  m_device->setWindowCaption(L"Irrlicht");
533 
534  ISceneManager* smgr = m_device->getSceneManager();
535  smgr->addLightSceneNode(0, vector3df(18,-12,6), SColorf(1.0, 1.0, 1.0), 30.0f);
536  smgr->addLightSceneNode(0, vector3df(-18,12,6), SColorf(1.0, 1.0, 1.0), 30.0f);
537  m_cnode = smgr->addCameraSceneNode();
538 #if 1
539  m_cnode->setTarget(vector3df(0,0,0.7));
540  m_cnode->setUpVector(vector3df(0,0,1));
541 #endif
542  m_receiver = new MyEventReceiver(m_cnode, 3);
543  m_device->setEventReceiver(m_receiver);
544  m_defaultCamera = new GLcamera(m_cnode);
545  setCamera(m_defaultCamera);
546 
547  return true;
548 }
549 
550 GLbody *GLscene::addBody(OpenHRP::BodyInfo_var i_binfo)
551 {
552  ISceneManager* smgr = m_device->getSceneManager();
553  return new GLbody(smgr->getRootSceneNode(), smgr, -1, i_binfo);
554 }
555 
556 void GLscene::setCamera(GLcamera *i_camera)
557 {
558  m_camera = i_camera;
559  m_camera->setCameraParameters(m_cnode);
560 }
561 
562 
564 {
565  return m_camera;
566 }
567 
568 GLcamera::GLcamera(ISceneNode *i_node) : m_node(i_node), m_near(0.1), m_far(100.0), m_fovy(M_PI/4), m_width(640), m_height(480)
569 {
570 }
571 
572 ISceneManager *GLscene::getSceneManager()
573 {
574  return m_device->getSceneManager();
575 }
576 
577 IVideoDriver *GLscene::getVideoDriver()
578 {
579  return m_device->getVideoDriver();
580 }
unsigned int height()
Definition: GLcamera.h:31
GLscene()
Definition: GLmodel.cpp:347
double * getAbsTransform()
Definition: GLcamera.cpp:108
unsigned int m_height
Definition: GLcamera.h:49
void init()
Definition: GLmodel.cpp:362
png_infop png_charp png_int_32 png_int_32 int * type
MyEventReceiver(ICameraSceneNode *i_camera, double i_radius=3.0)
Definition: IrrModel.cpp:17
void updateEye()
Definition: IrrModel.cpp:92
double near()
Definition: GLcamera.h:27
void addBody(GLbody *i_body)
Definition: GLmodel.cpp:293
png_infop png_charpp name
double m_near
Definition: GLcamera.h:48
void setCameraParameters(irr::scene::ICameraSceneNode *i_camera)
Definition: IrrModel.cpp:396
vector3df m_eye
Definition: IrrModel.cpp:101
const std::string & name() const
Definition: GLcamera.cpp:55
OpenHRP::matrix33 Matrix33
png_uint_32 i
double fovy()
Definition: GLcamera.h:29
irr::scene::ISceneNode * m_node
Definition: IrrModel.h:48
Definition: GLbody.h:11
GLcamera * getCamera()
Definition: GLmodel.cpp:429
Matrix33 rotFromRpy(const Vector3 &rpy)
static GLscene * getInstance()
Definition: GLmodel.cpp:332
OpenHRP::vector3 Vector3
static GLscene * m_scene
Definition: GLmodel.h:96
void updateAbsoluteTransformation(ISceneNode *i_node)
Definition: IrrModel.cpp:429
unsigned int width()
Definition: GLcamera.h:30
double m_fovy
Definition: GLcamera.h:48
list index
HRP_UTIL_EXPORT void calcRodrigues(Matrix44 &out_R, const Vector3 &axis, double q)
unsigned int m_width
Definition: GLcamera.h:49
GLcamera * findCamera(const char *i_name)
Definition: GLbody.cpp:71
t
irr::scene::ISceneManager * getSceneManager()
Definition: IrrModel.cpp:572
path
void updateCameraTransform(irr::scene::ICameraSceneNode *i_camera)
Definition: IrrModel.cpp:438
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
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
#define M_PI
void draw()
Definition: IrrModel.cpp:496
void setPosture(const double *i_angles)
Definition: GLbody.cpp:21
void setCamera(GLcamera *i_camera)
Definition: GLmodel.cpp:420
virtual bool OnEvent(const SEvent &event)
Definition: IrrModel.cpp:25
view
irr::video::IVideoDriver * getVideoDriver()
Definition: IrrModel.cpp:577
~GLscene()
Definition: GLmodel.cpp:357
GLbody()
Definition: GLbody.cpp:14
double m_far
Definition: GLcamera.h:48
ICameraSceneNode * m_camera
Definition: IrrModel.cpp:99
position2d< s32 > m_mouse
Definition: IrrModel.cpp:100
double far()
Definition: GLcamera.h:28


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