iCub-main
bvhnode.h
Go to the documentation of this file.
1 /*
2  * bvhnode.h
3  */
4 
5 /*
6  * Copyright (C) 2009 RobotCub Consortium
7  * Author: Alessandro Scalzo alessandro.scalzo@iit.it
8  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
9  *
10  * Based on:
11  *
12  * Qavimator
13  * Copyright (C) 2006 by Zi Ree *
14  * Zi Ree @ SecondLife *
15  * Released under the terms of the GNU GPL v2.0.
16  */
17 
18 #ifndef BVHNODE_H
19 #define BVHNODE_H
20 
21 //#include <QtCore>
22 
23 #ifdef __APPLE__
24 #include <OpenGL/glu.h>
25 #include <GLUT/glut.h>
26 #else
27 #if defined(WIN32) || defined(WIN64)
28 #include <windows.h>
29 #endif
30 #include <GL/glu.h>
31 #include <GL/glut.h>
32 #endif
33 
34 #include <vector>
35 #include <qstring.h>
36 #include <qslider.h>
37 
38 #include <QList>
39 
40 #include "mesh.h"
41 
42 #include <yarp/os/Network.h>
43 #include <yarp/os/BufferedPort.h>
44 #include <yarp/os/Bottle.h>
45 #include <yarp/os/Time.h>
46 
48 {
49 public:
50  ForceArrow(double x,double y,double z,double f,double fx,double fy,double fz,double mx,double my,double mz)
51  {
52  static const double dRad2Deg=180.0/M_PI;
53 
54  px=1000.0*x; py=1000.0*y; pz=1000.0*z;
55 
56  fth=0.0;
57  fax=0.0;
58  fay=0.0;
59  faz=1.0;
60 
61  bForce=(f>mForceThr);
62 
63  if (bForce)
64  {
65  fm=mForceGain*f-20.0;
66  if (fm<0.0) fm=0.0;
67 
68  nFSec=2+int(0.005*fm);
69 
70  double a=fx*fx+fy*fy;
71 
72  if (a>0.0)
73  {
74  a=sqrt(a);
75  fth=dRad2Deg*atan2(a,fz);
76  fax=-fy/a;
77  fay= fx/a;
78  faz= 0.0;
79  }
80  else if (fz<0.0)
81  {
82  fth=180.0;
83  fax=1.0;
84  fay=0.0;
85  faz=0.0;
86  }
87  }
88 
89  mth=0.0;
90  max=0.0;
91  may=0.0;
92  maz=1.0;
93 
94  double m=mx*mx+my*my+mz*mz;
95 
96  bTorque=(m>mTorqueThr);
97 
98  if (bTorque)
99  {
100  m=sqrt(m);
101 
102  mx/=m; my/=m; mz/=m;
103  mm=mTorqueGain*m-20.0;
104  if (mm<0.0) mm=0.0;
105 
106  nTSec=2+int(0.005*mm);
107 
108  double a=mx*mx+my*my;
109 
110  if (a>0.0)
111  {
112  a=sqrt(a);
113  mth=dRad2Deg*atan2(a,mz);
114  max=-my/a;
115  may= mx/a;
116  maz= 0.0;
117  }
118  else if (mz<0.0)
119  {
120  mth=180.0;
121  max=1.0;
122  may=0.0;
123  maz=0.0;
124  }
125  }
126 
127  gluQuadricDrawStyle(cyl=gluNewQuadric(),GLU_FILL);
128  }
130  {
131  if (cyl) gluDeleteQuadric(cyl);
132  }
133 
134  void draw()
135  {
136  if (!cyl) return;
137 
138  if (bForce)
139  {
140  glPushMatrix();
141  glColor4f(1.0f,0.0f,0.0f,1.0f);
142  glTranslated(px,py,pz);
143  glRotated(fth,fax,fay,faz);
144  glTranslated(0.0,0.0,-20.0); // cone base
145  glutSolidCone(5.0,20.0,16,4);
146  glTranslated(0.0,0.0,-fm);
147  gluCylinder(cyl,2.5,2.5,fm,16,nFSec);
148  glPopMatrix();
149  }
150 
151  if (bTorque)
152  {
153  glPushMatrix();
154  glColor4f(0.0f,0.0f,1.0f,1.0f);
155  glTranslated(px,py,pz);
156  glRotated(mth,max,may,maz);
157  glTranslated(0.0,0.0,-20.0); // cone base
158  glutSolidCone(5.0,20.0,16,4);
159  glTranslated(0.0,0.0,-mm);
160  gluCylinder(cyl,2.5,2.5,mm,16,nTSec);
161  glPopMatrix();
162  }
163  }
164 
165  static void setParams(double fg,double ft,double tg,double tt)
166  {
167  mForceGain=fg;
168  mForceThr=ft;
169  mTorqueGain=tg;
170  mTorqueThr=tt;
171  }
172 
173 protected:
174  GLUquadricObj *cyl;
175 
177 
178  double px,py,pz;
179 
180  double fm,fth,fax,fay,faz;
181  double mm,mth,max,may,maz;
182 
184 
185  static double mForceThr;
186  static double mForceGain;
187  static double mTorqueThr;
188  static double mTorqueGain;
189 };
190 
191 class BVHNode
192 {
193 public:
194  BVHNode(const QString& name,int enc=-1,iCubMesh *mesh=0)
195  {
196  children.clear();
197 
198  m_name=name;
199  nEnc=enc;
200  pMesh=mesh;
201  cyl=gluNewQuadric();
202  gluQuadricDrawStyle(cyl,GLU_FILL);
203 
204  m_Alpha=1.0;
205  }
206  virtual ~BVHNode()
207  {
208  for(int i=0; i<children.count(); ++i)
209  {
210  delete children[i];
211  }
212 
213  if (pMesh) delete pMesh;
214  gluDeleteQuadric(cyl);
215 
216  clearArrows();
217  }
218 
219  bool isValid(){ return cyl!=NULL; }
220 
221  const QString& name() const
222  {
223  return m_name;
224  }
225 
226  int numChildren() const
227  {
228  return children.count();
229  }
230 
231  void addChild(BVHNode* pChild)
232  {
233  children.append(pChild);
234  }
235 
236  virtual void draw(double *encoders,BVHNode* pSelected)=0;
237  virtual void setSliders(QSlider *rx,QSlider *ry,QSlider *rz,QSlider *px,QSlider *py,QSlider *pz){}
238 
239  void addArrow(ForceArrow *pArrow)
240  {
241  mArrows.push_back(pArrow);
242  }
243 
244  void clearArrows()
245  {
246  for (int a=0; a<(int)mArrows.size(); ++a)
247  {
248  if (mArrows[a]) delete mArrows[a];
249  }
250 
251  mArrows.clear();
252  }
253 
254 protected:
255  virtual void drawJoint()
256  {
257  glTranslated(0.0,0.0,-12.7);
258  gluDisk(cyl,0.0,10.16,16,16);
259  gluCylinder(cyl,10.16,10.16,25.4,16,16);
260  glTranslated(0.0,0.0,25.4);
261  gluDisk(cyl,0.0,10.16,16,16);
262  glTranslated(0.0,0.0,-12.7);
263  }
264 
265  void drawArrows()
266  {
267  for (int a=0; a<(int)mArrows.size(); ++a)
268  {
269  if (mArrows[a])
270  {
271  mArrows[a]->draw();
272  }
273  }
274  }
275 
276  void setName(const QString& name)
277  {
278  m_name=name;
279  }
280 
281  GLUquadricObj *cyl;
282  QString m_name;
283  QList<BVHNode*> children;
284 
285  int nEnc;
287 
288  float m_Alpha;
289 
290  std::vector<ForceArrow*> mArrows;
291 };
292 
293 #endif
#define M_PI
Definition: XSensMTx.cpp:24
const QString & name() const
Definition: bvhnode.h:221
virtual void draw(double *encoders, BVHNode *pSelected)=0
void addChild(BVHNode *pChild)
Definition: bvhnode.h:231
void setName(const QString &name)
Definition: bvhnode.h:276
float m_Alpha
Definition: bvhnode.h:288
BVHNode(const QString &name, int enc=-1, iCubMesh *mesh=0)
Definition: bvhnode.h:194
int numChildren() const
Definition: bvhnode.h:226
virtual void drawJoint()
Definition: bvhnode.h:255
iCubMesh * pMesh
Definition: bvhnode.h:286
virtual ~BVHNode()
Definition: bvhnode.h:206
bool isValid()
Definition: bvhnode.h:219
virtual void setSliders(QSlider *rx, QSlider *ry, QSlider *rz, QSlider *px, QSlider *py, QSlider *pz)
Definition: bvhnode.h:237
int nEnc
Definition: bvhnode.h:285
void drawArrows()
Definition: bvhnode.h:265
QList< BVHNode * > children
Definition: bvhnode.h:283
QString m_name
Definition: bvhnode.h:282
void addArrow(ForceArrow *pArrow)
Definition: bvhnode.h:239
std::vector< ForceArrow * > mArrows
Definition: bvhnode.h:290
void clearArrows()
Definition: bvhnode.h:244
GLUquadricObj * cyl
Definition: bvhnode.h:281
static double mForceGain
Definition: bvhnode.h:186
double fay
Definition: bvhnode.h:180
double max
Definition: bvhnode.h:181
double may
Definition: bvhnode.h:181
void draw()
Definition: bvhnode.h:134
double maz
Definition: bvhnode.h:181
double px
Definition: bvhnode.h:178
double fth
Definition: bvhnode.h:180
int nTSec
Definition: bvhnode.h:176
bool bForce
Definition: bvhnode.h:183
int nFSec
Definition: bvhnode.h:176
static double mTorqueThr
Definition: bvhnode.h:187
static void setParams(double fg, double ft, double tg, double tt)
Definition: bvhnode.h:165
double fax
Definition: bvhnode.h:180
static double mForceThr
Definition: bvhnode.h:185
double py
Definition: bvhnode.h:178
ForceArrow(double x, double y, double z, double f, double fx, double fy, double fz, double mx, double my, double mz)
Definition: bvhnode.h:50
double mth
Definition: bvhnode.h:181
double fm
Definition: bvhnode.h:180
double mm
Definition: bvhnode.h:181
double pz
Definition: bvhnode.h:178
bool bTorque
Definition: bvhnode.h:183
~ForceArrow()
Definition: bvhnode.h:129
double faz
Definition: bvhnode.h:180
static double mTorqueGain
Definition: bvhnode.h:188
GLUquadricObj * cyl
Definition: bvhnode.h:174
Definition: mesh.h:54