GLbody.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include "GLlink.h"
00003 #include "GLbody.h"
00004 
00005 using namespace hrp;
00006 
00007 bool GLbody::m_useAbsTransformToDraw = false;
00008 
00009 void GLbody::useAbsTransformToDraw()
00010 {
00011     m_useAbsTransformToDraw = true;
00012 }
00013 
00014 GLbody::GLbody()
00015 {
00016 }
00017 
00018 GLbody::~GLbody(){
00019 }
00020 
00021 void GLbody::setPosture(const double *i_angles){
00022     for (unsigned int i=0; i<numLinks(); i++){
00023         ((GLlink *)link(i))->setQ(i_angles[i]);
00024     }
00025 }
00026 
00027 void GLbody::setPosition(double x, double y, double z)
00028 {
00029     ((GLlink *)rootLink())->setPosition(x,y,z);
00030 }
00031 
00032 void GLbody::setRotation(double r, double p, double y)
00033 {
00034     ((GLlink *)rootLink())->setRotation(r,p,y);
00035 }
00036 
00037 void GLbody::setRotation(const double *R)
00038 {
00039     ((GLlink *)rootLink())->setRotation(R);
00040 }
00041 
00042 void GLbody::setPosture(const double *i_angles, double *i_pos, double *i_rpy){
00043     setPosition(i_pos[0], i_pos[1], i_pos[2]); 
00044     setRotation(i_rpy[0], i_rpy[1], i_rpy[2]); 
00045     setPosture(i_angles);
00046 }
00047 
00048 void GLbody::setPosture(const dvector& i_q, const Vector3& i_p,
00049                         const Matrix33& i_R)
00050 {
00051     double *tform = ((GLlink *)rootLink())->getTransform();
00052     tform[ 0]=i_R(0,0);tform[ 1]=i_R(1,0);tform[ 2]=i_R(2,0);tform[ 3]=0;
00053     tform[ 4]=i_R(0,1);tform[ 5]=i_R(1,1);tform[ 6]=i_R(2,1);tform[ 7]=0;
00054     tform[ 8]=i_R(0,2);tform[ 9]=i_R(1,2);tform[10]=i_R(2,2);tform[11]=0;
00055     tform[12]=i_p[0];tform[13]=i_p[1];tform[14]=i_p[2];tform[15]=1;
00056     setPosture(i_q.data());
00057 }
00058 
00059 size_t GLbody::draw(){
00060     size_t ntri=0;
00061     if (m_useAbsTransformToDraw){
00062         for (unsigned int i=0; i<numLinks(); i++){
00063             ntri += ((GLlink *)link(i))->draw();
00064         }
00065     }else{
00066         ntri = ((GLlink *)rootLink())->draw(); // drawn recursively
00067     }
00068     return ntri;
00069 }
00070 
00071 GLcamera *GLbody::findCamera(const char *i_name){
00072     for (unsigned int i=0; i<numLinks(); i++){
00073         GLcamera *camera = ((GLlink *)link(i))->findCamera(i_name);
00074         if (camera) return camera;
00075     }
00076     return NULL;
00077 }
00078 
00079 void GLbody::drawSensor(Sensor *sensor)
00080 {
00081     if (m_sensorDrawCallback) m_sensorDrawCallback(this, sensor);
00082 }
00083 
00084 void GLbody::setSensorDrawCallback(boost::function2<void, hrp::Body *, hrp::Sensor *> f)
00085 {
00086     m_sensorDrawCallback = f;
00087 }
00088 
00089 boost::function2<void, hrp::Body *, hrp::Sensor *> GLbody::getSensorDrawCallback()
00090 {
00091     return m_sensorDrawCallback;
00092 }
00093 
00094 void GLbody::divideLargeTriangles(double maxEdgeLen)
00095 {
00096     for (unsigned int i=0; i<numLinks(); i++){
00097         GLlink *l = (GLlink *)link(i);
00098         l->divideLargeTriangles(maxEdgeLen);
00099     }
00100 }
00101 
00102 void GLbody::computeAABB(hrp::Vector3& o_min, hrp::Vector3& o_max)
00103 {
00104     if (m_useAbsTransformToDraw){
00105         hrp::Vector3 mi, ma;
00106         for (unsigned int i=0; i<numLinks(); i++){
00107             ((GLlink *)link(i))->computeAABB(mi,ma);
00108             if (i==0){
00109                 o_min = mi; o_max = ma;
00110             }else{
00111                 for (int j=0; j<3; j++){
00112                     if (o_min[j] > mi[j]) o_min[j] = mi[j];
00113                     if (o_max[j] < ma[j]) o_max[j] = ma[j];
00114                 }
00115             }
00116         }
00117     }else{
00118         // TODO implement
00119     }
00120 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55