GLbody.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include "GLlink.h"
3 #include "GLbody.h"
4 
5 using namespace hrp;
6 
8 
10 {
11  m_useAbsTransformToDraw = true;
12 }
13 
15 {
16 }
17 
19 }
20 
21 void GLbody::setPosture(const double *i_angles){
22  for (unsigned int i=0; i<numLinks(); i++){
23  ((GLlink *)link(i))->setQ(i_angles[i]);
24  }
25 }
26 
27 void GLbody::setPosition(double x, double y, double z)
28 {
29  ((GLlink *)rootLink())->setPosition(x,y,z);
30 }
31 
32 void GLbody::setRotation(double r, double p, double y)
33 {
34  ((GLlink *)rootLink())->setRotation(r,p,y);
35 }
36 
37 void GLbody::setRotation(const double *R)
38 {
39  ((GLlink *)rootLink())->setRotation(R);
40 }
41 
42 void GLbody::setPosture(const double *i_angles, double *i_pos, double *i_rpy){
43  setPosition(i_pos[0], i_pos[1], i_pos[2]);
44  setRotation(i_rpy[0], i_rpy[1], i_rpy[2]);
45  setPosture(i_angles);
46 }
47 
48 void GLbody::setPosture(const dvector& i_q, const Vector3& i_p,
49  const Matrix33& i_R)
50 {
51  double *tform = ((GLlink *)rootLink())->getTransform();
52  tform[ 0]=i_R(0,0);tform[ 1]=i_R(1,0);tform[ 2]=i_R(2,0);tform[ 3]=0;
53  tform[ 4]=i_R(0,1);tform[ 5]=i_R(1,1);tform[ 6]=i_R(2,1);tform[ 7]=0;
54  tform[ 8]=i_R(0,2);tform[ 9]=i_R(1,2);tform[10]=i_R(2,2);tform[11]=0;
55  tform[12]=i_p[0];tform[13]=i_p[1];tform[14]=i_p[2];tform[15]=1;
56  setPosture(i_q.data());
57 }
58 
59 size_t GLbody::draw(){
60  size_t ntri=0;
61  if (m_useAbsTransformToDraw){
62  for (unsigned int i=0; i<numLinks(); i++){
63  ntri += ((GLlink *)link(i))->draw();
64  }
65  }else{
66  ntri = ((GLlink *)rootLink())->draw(); // drawn recursively
67  }
68  return ntri;
69 }
70 
71 GLcamera *GLbody::findCamera(const char *i_name){
72  for (unsigned int i=0; i<numLinks(); i++){
73  GLcamera *camera = ((GLlink *)link(i))->findCamera(i_name);
74  if (camera) return camera;
75  }
76  return NULL;
77 }
78 
80 {
81  if (m_sensorDrawCallback) m_sensorDrawCallback(this, sensor);
82 }
83 
84 void GLbody::setSensorDrawCallback(boost::function2<void, hrp::Body *, hrp::Sensor *> f)
85 {
86  m_sensorDrawCallback = f;
87 }
88 
89 boost::function2<void, hrp::Body *, hrp::Sensor *> GLbody::getSensorDrawCallback()
90 {
91  return m_sensorDrawCallback;
92 }
93 
94 void GLbody::divideLargeTriangles(double maxEdgeLen)
95 {
96  for (unsigned int i=0; i<numLinks(); i++){
97  GLlink *l = (GLlink *)link(i);
98  l->divideLargeTriangles(maxEdgeLen);
99  }
100 }
101 
103 {
104  if (m_useAbsTransformToDraw){
105  hrp::Vector3 mi, ma;
106  for (unsigned int i=0; i<numLinks(); i++){
107  ((GLlink *)link(i))->computeAABB(mi,ma);
108  if (i==0){
109  o_min = mi; o_max = ma;
110  }else{
111  for (int j=0; j<3; j++){
112  if (o_min[j] > mi[j]) o_min[j] = mi[j];
113  if (o_max[j] < ma[j]) o_max[j] = ma[j];
114  }
115  }
116  }
117  }else{
118  // TODO implement
119  }
120 }
size_t draw()
Definition: GLbody.cpp:59
void divideLargeTriangles(double maxEdgeLen)
Definition: GLbody.cpp:94
void setPosition(double x, double y, double z)
Definition: GLbody.cpp:27
~GLbody()
Definition: GLbody.cpp:18
boost::function2< void, hrp::Body *, hrp::Sensor * > getSensorDrawCallback()
Definition: GLbody.cpp:89
static bool m_useAbsTransformToDraw
Definition: GLbody.h:37
OpenHRP::matrix33 Matrix33
png_uint_32 i
Eigen::VectorXd dvector
void computeAABB(hrp::Vector3 &o_min, hrp::Vector3 &o_max)
Definition: GLbody.cpp:102
Eigen::Vector3d Vector3
void setRotation(const double *R)
Definition: GLbody.cpp:37
OpenHRP::vector3 Vector3
GLcamera * findCamera(const char *i_name)
Definition: GLbody.cpp:71
static void useAbsTransformToDraw()
Definition: GLbody.cpp:9
void setPosture(const double *i_angles)
Definition: GLbody.cpp:21
void setSensorDrawCallback(boost::function2< void, hrp::Body *, hrp::Sensor *> f)
Definition: GLbody.cpp:84
void drawSensor(hrp::Sensor *i_sensor)
Definition: GLbody.cpp:79
GLbody()
Definition: GLbody.cpp:14


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