GLlink.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #ifdef __APPLE__
3 #include <OpenGL/gl.h>
4 #else
5 #include <GL/gl.h>
6 #endif
7 #include <hrpModel/Body.h>
8 #include <hrpModel/Sensor.h>
9 #include <hrpModel/Light.h>
10 #include "GLutil.h"
11 #include "GLbody.h"
12 #include "GLcamera.h"
13 #include "GLlink.h"
14 #include "GLshape.h"
15 
16 using namespace OpenHRP;
17 using namespace hrp;
18 
21 
23 {
24  m_useAbsTransformToDraw = true;
25 }
26 
27 GLlink::GLlink() : m_showAxes(false), m_highlight(false)
28 {
29  Rs = hrp::Matrix33::Identity();
30  R = hrp::Matrix33::Identity();
31  setQ(0);
32 }
33 
35 {
36  for (size_t i=0; i<m_shapes.size(); i++){
37  delete m_shapes[i];
38  }
39  for (unsigned int i=0; i<m_cameras.size(); i++){
40  delete m_cameras[i];
41  }
42 }
43 
44 size_t GLlink::draw(){
45  size_t ntri=0;
46  glPushMatrix();
48  glMultMatrixd(m_absTrans);
49  }else{
50  glMultMatrixd(m_trans);
51  glMultMatrixd(m_T_j);
52  }
53  if (m_drawMode != DM_COLLISION){
54  for (size_t i=0; i<m_shapes.size(); i++){
55  ntri += m_shapes[i]->draw(m_drawMode);
56  }
57  for (size_t i=0; i<m_cameras.size(); i++){
58  ntri += m_cameras[i]->draw(m_drawMode);
59  }
60  }else{
61  if (coldetModel && coldetModel->getNumTriangles()){
62  ntri = coldetModel->getNumTriangles();
63  Eigen::Vector3f n, v[3];
64  int vindex[3];
65  if (m_highlight){
66  float red[] = {1,0,0,1};
67  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, red);
68  }else{
69  float gray[] = {0.8,0.8,0.8,1};
70  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, gray);
71  }
72  glBegin(GL_TRIANGLES);
73  for (int i=0; i<coldetModel->getNumTriangles(); i++){
74  coldetModel->getTriangle(i, vindex[0], vindex[1], vindex[2]);
75  for (int j=0; j<3; j++){
76  coldetModel->getVertex(vindex[j], v[j][0], v[j][1], v[j][2]);
77  }
78  n = (v[1]-v[0]).cross(v[2]-v[0]);
79  n.normalize();
80  glNormal3fv(n.data());
81  for (int j=0; j<3; j++){
82  glVertex3fv(v[j].data());
83  }
84  }
85  glEnd();
86  }
87  }
88  for (size_t i=0; i<sensors.size(); i++){
89  Sensor *s = sensors[i];
90  double T[16];
91  for (int i=0; i<3; i++){
92  for (int j=0; j<3; j++){
93  T[i*4+j] = s->localR(j,i);
94  }
95  }
96  T[12] = s->localPos[0]; T[13] = s->localPos[1]; T[14] = s->localPos[2];
97  T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
98  glPushMatrix();
99  glMultMatrixd(T);
100  GLbody *glb = dynamic_cast<GLbody *>(body);
101  glb->drawSensor(s);
102  glPopMatrix();
103  }
104  for (size_t i=0; i<lights.size(); i++){
105  Light *l = lights[i];
106  int lid = GL_LIGHT0+l->id;
107  if (!l->on){
108  glDisable(lid);
109  continue;
110  }
111  double T[16];
112  for (int j=0; j<3; j++){
113  for (int k=0; k<3; k++){
114  T[j*4+k] = l->localR(k,j);
115  }
116  }
117  T[12] = l->localPos[0]; T[13] = l->localPos[1]; T[14] = l->localPos[2];
118  T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
119  glPushMatrix();
120  glMultMatrixd(T);
121  glEnable(GL_LIGHTING);
122  glEnable(lid);
123  GLfloat pos[] = {0,0,0,1};
124  glLightfv(lid, GL_POSITION, pos);
125  GLfloat color[] = {l->color[0], l->color[1], l->color[2], 1};
126  glLightfv(lid, GL_DIFFUSE, color);
127  if (l->type == POINT){
128  glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]);
129  glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]);
130  glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
131  }else if(l->type == SPOT){
132  glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]);
133  glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]);
134  glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
135  glLightf(lid, GL_SPOT_EXPONENT, 20);
136  glLightf(lid, GL_SPOT_CUTOFF, l->cutOffAngle*180/M_PI);
137  GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
138  glLightfv(lid, GL_SPOT_DIRECTION, dir);
139  }else if(l->type == DIRECTIONAL){
140  GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
141  glLightfv(lid, GL_SPOT_DIRECTION, dir);
142  }
143  glPopMatrix();
144  }
145 
146  if (m_showAxes){
147  glDisable(GL_LIGHTING);
148  glBegin(GL_LINES);
149  glColor3f(1,0,0);
150  glVertex3f(0,0,0); glVertex3f(0.5, 0, 0);
151  glColor3f(0,1,0);
152  glVertex3f(0,0,0); glVertex3f(0, 0.5, 0);
153  glColor3f(0,0,1);
154  glVertex3f(0,0,0); glVertex3f(0, 0, 0.5);
155  glEnd();
156  glEnable(GL_LIGHTING);
157  }
159  hrp::Link *l = child;
160  while (l){
161  ntri += ((GLlink *)l)->draw();
162  l = l->sibling;
163  }
164  }
165  glPopMatrix();
166  return ntri;
167 }
168 
169 void GLlink::setQ(double i_q){
170  switch(jointType){
171  case ROTATIONAL_JOINT:
172  {
173  Matrix33 R;
174  Vector3 aLocal(Rs.transpose()*a);
175  hrp::calcRodrigues(R, aLocal, i_q);
176  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;
177  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;
178  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;
179  m_T_j[12]=0; m_T_j[13]=0; m_T_j[14]=0; m_T_j[15]=1;
180  }
181  break;
182  case SLIDE_JOINT:
183  {
184  Vector3 aLocal(Rs.transpose()*d);
185  m_T_j[ 0]=1;m_T_j[ 1]=0;m_T_j[ 2]=0;m_T_j[ 3]=0;
186  m_T_j[ 4]=0;m_T_j[ 5]=1;m_T_j[ 6]=0;m_T_j[ 7]=0;
187  m_T_j[ 8]=0;m_T_j[ 9]=0;m_T_j[10]=1;m_T_j[11]=0;
188  m_T_j[12]=aLocal[0]*i_q;
189  m_T_j[13]=aLocal[1]*i_q;
190  m_T_j[14]=aLocal[2]*i_q;
191  m_T_j[15]=1;
192  }
193  break;
194  default:
195  m_T_j[ 0]=1;m_T_j[ 1]=0;m_T_j[ 2]=0;m_T_j[ 3]=0;
196  m_T_j[ 4]=0;m_T_j[ 5]=1;m_T_j[ 6]=0;m_T_j[ 7]=0;
197  m_T_j[ 8]=0;m_T_j[ 9]=0;m_T_j[10]=1;m_T_j[11]=0;
198  m_T_j[12]=0;m_T_j[13]=0;m_T_j[14]=0;m_T_j[15]=1;
199  break;
200  }
201  //printf("m_T_j:\n");
202  //printMatrix(m_T_j);
203 }
204 
205 void GLlink::setAbsTransform(double i_trans[16]){
206  memcpy(m_absTrans, i_trans, sizeof(double)*16);
207 }
208 
209 GLcamera *GLlink::findCamera(const char *i_name){
210  std::string name(i_name);
211  for (unsigned int i=0; i<m_cameras.size(); i++){
212  if (m_cameras[i]->name() == name) return m_cameras[i];
213  }
214  return NULL;
215 }
216 
219 }
220 
221 void GLlink::computeAbsTransform(double o_trans[16]){
223  memcpy(o_trans, m_absTrans, sizeof(double)*16);
224  return;
225  }
226  if (parent){
227  double trans1[16], trans2[16];
228  mulTrans(m_T_j, m_trans, trans1);
229  ((GLlink *)parent)->computeAbsTransform(trans2);
230  mulTrans(trans1, trans2, o_trans);
231  }else{
232  memcpy(o_trans, m_trans, sizeof(double)*16);
233  }
234 }
235 
237 {
238  m_shapes.push_back(shape);
239 }
240 
242 {
243  m_cameras.push_back(camera);
244 }
245 
246 void GLlink::showAxes(bool flag)
247 {
248  m_showAxes = flag;
249 }
250 
252 {
253  return new GLlink();
254 }
255 
256 const std::vector<GLcamera *>& GLlink::cameras()
257 {
258  return m_cameras;
259 }
260 
262 {
263  return m_drawMode;
264 }
265 
266 void GLlink::drawMode(int i_mode)
267 {
268  m_drawMode = i_mode;
269 }
270 
271 void GLlink::highlight(bool flag)
272 {
273  m_highlight = flag;
274  for (size_t i=0; i<m_shapes.size(); i++){
275  m_shapes[i]->highlight(flag);
276  }
277  for (size_t i=0; i<m_cameras.size(); i++){
278  m_cameras[i]->highlight(flag);
279  }
280 }
281 
282 void GLlink::divideLargeTriangles(double maxEdgeLen)
283 {
284  for (size_t i=0; i<m_shapes.size(); i++){
285  m_shapes[i]->divideLargeTriangles(maxEdgeLen);
286  }
287 }
288 
290 {
292  hrp::Vector3 mi, ma, p(m_absTrans[12], m_absTrans[13], m_absTrans[14]);
294  for (int i=0; i<3; i++){
295  for (int j=0; j<3; j++){
296  R(j,i) = m_absTrans[i*4+j];
297  }
298  }
299  for (size_t i=0; i<m_shapes.size(); i++){
300  m_shapes[i]->computeAABB(p, R, mi, ma);
301  if (i==0){
302  o_min = mi; o_max = ma;
303  }else{
304  for (int j=0; j<3; j++){
305  if (o_min[j] > mi[j]) o_min[j] = mi[j];
306  if (o_max[j] < ma[j]) o_max[j] = ma[j];
307  }
308  }
309  }
310  }else{
311  // TODO: implement
312  //glMultMatrixd(m_trans);
313  //glMultMatrixd(m_T_j);
314  }
315 }
Vector3 direction
OpenHRP::matrix33 Matrix33
png_uint_32 i
Matrix33 localR
Definition: GLbody.h:11
Eigen::Vector3d Vector3
Eigen::Matrix3d Matrix33
OpenHRP::vector3 Vector3
Matrix33 localR
void red()
Vector3 localPos
HRP_UTIL_EXPORT void calcRodrigues(Matrix44 &out_R, const Vector3 &axis, double q)
n
Vector3 attenuation
Vector3 color
void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
Definition: GLutil.cpp:58
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
#define M_PI
JSAMPIMAGE data
void drawSensor(hrp::Sensor *i_sensor)
Definition: GLbody.cpp:79
Vector3 localPos
double cutOffAngle


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