GLutil.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <math.h>
3 #ifdef __APPLE__
4 #include <OpenGL/gl.h>
5 #include <OpenGL/glu.h>
6 #else
7 #include <GL/gl.h>
8 #include <GL/glu.h>
9 #endif
10 #include <hrpUtil/Eigen3d.h>
11 #include "GLshape.h"
12 #include "GLbody.h"
13 #include "GLlink.h"
14 #include "GLcamera.h"
15 #include "GLtexture.h"
16 #include "GLutil.h"
17 
18 using namespace OpenHRP;
19 using namespace hrp;
20 
22 public:
23  void setShapeSetInfo(ShapeSetInfo_ptr i_ssinfo);
24  void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo, GLshape *(*shapeFactory)());
25  void loadShapeFromLinkInfo(GLlink *link, const LinkInfo &i_li, GLshape *(*shapeFactory)());
26  void loadShape(GLshape *shape,
27  const OpenHRP::TransformedShapeIndex &i_tsi);
28  void loadCamera(GLcamera *camera, const OpenHRP::SensorInfo &i_si);
29  ShapeInfoSequence_var sis;
30  AppearanceInfoSequence_var ais;
31  MaterialInfoSequence_var mis;
32  TextureInfoSequence_var txs;
33 };
34 
35 void shapeLoader::setShapeSetInfo(ShapeSetInfo_ptr i_ssinfo)
36 {
37  sis = i_ssinfo->shapes();
38  ais = i_ssinfo->appearances();
39  mis = i_ssinfo->materials();
40  txs = i_ssinfo->textures();
41 }
42 
43 void shapeLoader::loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo,
44  GLshape *(*shapeFactory)())
45 {
46  LinkInfoSequence_var lis = i_binfo->links();
47  for (unsigned int i=0; i<lis->length(); i++){
48  hrp::Link *l = body->link(std::string(lis[i].name));
49  if (l){
50  loadShapeFromLinkInfo((GLlink *)l, lis[i], shapeFactory);
51  }else{
52  std::cout << "can't find a link named " << lis[i].name
53  << std::endl;
54  }
55  }
56 }
57 
58 void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
59 {
60  for (int i=0; i<4; i++){
61  for (int j=0; j<4;j++){
62  double v = 0;
63  for (int k=0; k<4; k++){
64  v += i_m1[i*4+k]*i_m2[j+k*4];
65  }
66  o_m[i*4+j] = v;
67  }
68  }
69 }
70 
71 bool loadTextureFromTextureInfo(GLtexture *texture, TextureInfo &ti)
72 {
73  if (ti.image.length() == 0){
74  std::cerr << "texture image is not loaded(" << ti.url << ")" << std::endl;
75  return false;
76  }else if (ti.numComponents != 3 && ti.numComponents != 4){
77  std::cerr << "texture image which has "
78  << ti.numComponents
79  << " components is not supported(" << ti.url << ")"
80  << std::endl;
81  return false;
82  }
83  texture->repeatS = ti.repeatS;
84  texture->repeatT = ti.repeatT;
85  texture->numComponents = ti.numComponents;
86  texture->url = ti.url;
87  texture->width = ti.width;
88  texture->height = ti.height;
89  texture->image.resize(ti.image.length());
90  memcpy(&texture->image[0], ti.image.get_buffer(), ti.image.length());
91  return true;
92 }
93 
95  const OpenHRP::TransformedShapeIndex &i_tsi)
96 {
97  shape->setTransform(i_tsi.transformMatrix);
98  ShapeInfo& si = sis[i_tsi.shapeIndex];
99  shape->setVertices(si.vertices.length()/3, si.vertices.get_buffer());
100  shape->setTriangles(si.triangles.length()/3,
101  (int *)si.triangles.get_buffer());
102  const AppearanceInfo& ai = ais[si.appearanceIndex];
103  shape->setNormals(ai.normals.length()/3, ai.normals.get_buffer());
104  shape->setNormalIndices(ai.normalIndices.length(),
105  (int *)ai.normalIndices.get_buffer());
106  shape->setTextureCoordinates(ai.textureCoordinate.length()/2,
107  ai.textureCoordinate.get_buffer());
108  shape->setTextureCoordIndices(ai.textureCoordIndices.length(),
109  (int *)ai.textureCoordIndices.get_buffer());
110  shape->setColors(ai.colors.length()/3, ai.colors.get_buffer());
111  if (ai.textureIndex >=0){
112  if (txs->length() <= (unsigned int)ai.textureIndex){
113  std::cerr << "invalid texture index(" << ai.textureIndex << ")"
114  << std::endl;
115  }else{
116  TextureInfo &ti = txs[ai.textureIndex];
117  GLtexture *texture = new GLtexture();
118  if (loadTextureFromTextureInfo(texture, ti)){
119  shape->setTexture(texture);
120  }else{
121  delete texture;
122  }
123  }
124  }
125  if (ai.colors.length()){
126  shape->setDiffuseColor(ai.colors[0], ai.colors[1], ai.colors[2], 1.0);
127  }else if (ai.materialIndex >= 0){
128  const MaterialInfo& mi = mis[ai.materialIndex];
129  shape->setDiffuseColor(mi.diffuseColor[0], mi.diffuseColor[1], mi.diffuseColor[2], 1.0-mi.transparency);
130  shape->setShininess(mi.shininess);
131  shape->setSpecularColor(mi.specularColor[0], mi.specularColor[1], mi.specularColor[2]);
132  }else{
133  //std::cout << "no material" << std::endl;
134  }
135  shape->normalPerVertex(ai.normalPerVertex);
136  shape->solid(ai.solid);
137  shape->compile();
138 }
139 
140 
141 void loadCube(GLshape *shape, double x, double y, double z)
142 {
143  double hx = x/2, hy = y/2, hz = z/2;
144  float vertices[] = {hx,hy,hz,
145  -hx,hy,hz,
146  -hx,-hy,hz,
147  hx,-hy,hz,
148  hx,hy,-hz,
149  -hx,hy,-hz,
150  -hx,-hy,-hz,
151  hx,-hy,-hz};
152  int triangles[] = {0,2,3,//+z
153  0,1,2,//+z
154  4,3,7,//+x
155  4,0,3,//+x
156  0,4,5,//+y
157  5,1,0,//+y
158  1,5,6,//-x
159  1,6,2,//-x
160  2,6,7,//-y
161  2,7,3,//-y
162  7,6,4,//-z
163  5,4,6};//-z
164  float normals[] = {1,0,0,
165  0,1,0,
166  0,0,1,
167  -1,0,0,
168  0,-1,0,
169  0,0,-1};
170  int normalIndices[] = {2,2,0,0,1,1,3,3,4,4,5,5};
171  shape->setVertices(8, vertices);
172  shape->setTriangles(12, triangles);
173  shape->setNormals(6, normals);
174  shape->setNormalIndices(12, normalIndices);
175  shape->setDiffuseColor(0.8, 0.8, 0.8, 1.0);
176  shape->normalPerVertex(false);
177  shape->solid(true);
178  shape->compile();
179 }
180 
181 void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo,
182  GLshape *(*shapeFactory)())
183 {
185  loader.setShapeSetInfo(i_binfo);
186  loader.loadShapeFromBodyInfo(body, i_binfo, shapeFactory);
187 }
188 
189 void loadShapeFromSceneInfo(GLlink *link, SceneInfo_var i_sinfo,
190  GLshape *(*shapeFactory)())
191 {
193  loader.setShapeSetInfo(i_sinfo);
194  TransformedShapeIndexSequence_var tsis = i_sinfo->shapeIndices();
195  for (size_t i = 0; i<tsis->length(); i++){
196  GLshape *shape = shapeFactory ? shapeFactory() : new GLshape();
197  loader.loadShape(shape, tsis[i]);
198  link->addShape(shape);
199  }
200 }
201 
203  const OpenHRP::LinkInfo &i_li,
204  OpenHRP::ShapeSetInfo_ptr i_ssinfo,
205  GLshape *(*shapeFactory)())
206 {
208  loader.setShapeSetInfo(i_ssinfo);
209  loader.loadShapeFromLinkInfo(link, i_li, shapeFactory);
210 }
211 
212 void shapeLoader::loadShapeFromLinkInfo(GLlink *link, const LinkInfo &i_li, GLshape *(*shapeFactory)()){
213  Vector3 axis;
214  Matrix33 R;
215 
216  for (int i=0; i<3; i++) axis[i] = i_li.rotation[i];
217  hrp::calcRodrigues(R, axis, i_li.rotation[3]);
218 
219  double trans[16];
220  trans[ 0]=R(0,0);trans[ 1]=R(0,1);trans[ 2]=R(0,2);trans[3]=i_li.translation[0];
221  trans[ 4]=R(1,0);trans[ 5]=R(1,1);trans[ 6]=R(1,2);trans[7]=i_li.translation[1];
222  trans[ 8]=R(2,0);trans[ 9]=R(2,1);trans[10]=R(2,2);trans[11]=i_li.translation[2];
223  link->setTransform(trans);
224  link->setQ(0);
225  link->computeAbsTransform();
226 
227  for (size_t i = 0; i<i_li.shapeIndices.length(); i++){
228  GLshape *shape = shapeFactory ? shapeFactory() : new GLshape();
229  loadShape(shape, i_li.shapeIndices[i]);
230  link->addShape(shape);
231  }
232 
233  const SensorInfoSequence& sensors = i_li.sensors;
234  for (unsigned int i=0; i<sensors.length(); i++){
235  const SensorInfo& si = sensors[i];
236  std::string type(si.type);
237  if (type == "Vision"){
238  //std::cout << si.name << std::endl;
239  GLcamera *camera = new GLcamera(si.specValues[4], si.specValues[5],
240  si.specValues[0], si.specValues[1],
241  si.specValues[2],
242  link, si.id);
243  loadCamera(camera, si);
244  link->addCamera(camera);
245  }else{
246  Vector3 p;
247  p[0] = si.translation[0];
248  p[1] = si.translation[1];
249  p[2] = si.translation[2];
250  Matrix33 R;
251  Vector3 axis;
252  axis[0] = si.rotation[0];
253  axis[1] = si.rotation[1];
254  axis[2] = si.rotation[2];
255  hrp::calcRodrigues(R, axis, si.rotation[3]);
256 
257  for (size_t i=0; i<si.shapeIndices.length(); i++){
258  GLshape *shape = shapeFactory ? shapeFactory() : new GLshape();
259  loadShape(shape, si.shapeIndices[i]);
260  Vector3 newp = R*shape->getPosition()+p;
261  shape->setPosition(newp[0], newp[1], newp[2]);
262  Matrix33 newR = R*shape->getRotation();
263  shape->setRotation(newR);
264  link->addShape(shape);
265  }
266  }
267  }
268 }
269 
270 void shapeLoader::loadCamera(GLcamera *i_camera, const SensorInfo &i_si)
271 {
272  i_camera->name(std::string(i_si.name));
273  i_camera->setPosition(i_si.translation[0], i_si.translation[1],
274  i_si.translation[2]);
275  i_camera->setRotation(i_si.rotation[0], i_si.rotation[1],
276  i_si.rotation[2], i_si.rotation[3]);
277 
278  for (size_t i=0; i<i_si.shapeIndices.length(); i++){
279  GLshape *shape = new GLshape();
280  loadShape(shape, i_si.shapeIndices[i]);
281  i_camera->addShape(shape);
282  }
283 }
void loadCamera(GLcamera *camera, const OpenHRP::SensorInfo &i_si)
Definition: GLutil.cpp:270
TextureInfoSequence_var txs
Definition: GLutil.cpp:32
png_infop png_charp png_int_32 png_int_32 int * type
void compile()
Definition: GLshape.cpp:129
void setNormals(unsigned int nnormal, const float *normals)
Definition: GLshape.cpp:64
MaterialInfoSequence_var mis
Definition: GLutil.cpp:31
void setTexture(GLtexture *texture)
Definition: GLshape.cpp:124
void setTextureCoordinates(unsigned int len, const float *coordinates)
Definition: GLshape.cpp:107
void addShape(GLshape *i_shape)
Definition: GLcamera.cpp:276
png_infop png_charpp name
void setTextureCoordIndices(unsigned int len, const int *coordinates)
Definition: GLshape.cpp:116
bool repeatS
Definition: GLtexture.h:9
void setShapeSetInfo(ShapeSetInfo_ptr i_ssinfo)
Definition: GLutil.cpp:35
void setSpecularColor(float r, float g, float b)
Definition: GLshape.cpp:269
int height
Definition: GLtexture.h:8
const std::string & name() const
Definition: GLcamera.cpp:55
Link * link(int index) const
void setVertices(unsigned int nvertices, const float *vertices)
Definition: GLshape.cpp:44
void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:181
OpenHRP::matrix33 Matrix33
png_uint_32 i
hrp::Matrix33 getRotation()
void setRotation(double r, double p, double y)
png_infop png_bytep * trans
void loadCube(GLshape *shape, double x, double y, double z)
Definition: GLutil.cpp:141
void loadShape(GLshape *shape, const OpenHRP::TransformedShapeIndex &i_tsi)
Definition: GLutil.cpp:94
void setTransform(const double i_trans[12])
Definition: GLbody.h:11
hrp::Vector3 getPosition()
void loadShapeFromLinkInfo(GLlink *link, const LinkInfo &i_li, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:212
bool loadTextureFromTextureInfo(GLtexture *texture, TextureInfo &ti)
Definition: GLutil.cpp:71
std::string url
Definition: GLtexture.h:11
void solid(bool flag)
Definition: GLshape.cpp:94
OpenHRP::vector3 Vector3
int numComponents
Definition: GLtexture.h:7
int width
Definition: GLtexture.h:8
void setShininess(float s)
Definition: GLshape.cpp:264
void setPosition(double x, double y, double z)
void setColors(unsigned int ncolors, const float *colors)
Definition: GLshape.cpp:79
HRP_UTIL_EXPORT void calcRodrigues(Matrix44 &out_R, const Vector3 &axis, double q)
void loadShapeFromSceneInfo(GLlink *link, SceneInfo_var i_sinfo, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:189
AppearanceInfoSequence_var ais
Definition: GLutil.cpp:30
void setTriangles(unsigned int ntriangles, const int *vertexIndices)
Definition: GLshape.cpp:54
void setDiffuseColor(float r, float g, float b, float a)
Definition: GLshape.cpp:74
void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
Definition: GLutil.cpp:58
typedef int
void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:43
void loadShapeFromLinkInfo(GLlink *link, const OpenHRP::LinkInfo &i_li, OpenHRP::ShapeSetInfo_ptr i_ssinfo, GLshape *(*shapeFactory)())
Definition: GLutil.cpp:202
void setNormalIndices(unsigned int len, const int *normalIndices)
Definition: GLshape.cpp:99
bool repeatT
Definition: GLtexture.h:9
std::vector< unsigned char > image
Definition: GLtexture.h:10
void normalPerVertex(bool flag)
Definition: GLshape.cpp:89
ShapeInfoSequence_var sis
Definition: GLutil.cpp:29


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