size_t GLlink::draw(){
size_t ntri=0;
glPushMatrix();
if (m_useAbsTransformToDraw){
glMultMatrixd(m_absTrans);
}else{
glMultMatrixd(m_trans);
glMultMatrixd(m_T_j);
}
if (m_drawMode != DM_COLLISION){
for (size_t i=0; i<m_shapes.size(); i++){
ntri += m_shapes[i]->draw(m_drawMode);
}
for (size_t i=0; i<m_cameras.size(); i++){
ntri += m_cameras[i]->draw(m_drawMode);
}
}else{
if (coldetModel && coldetModel->getNumTriangles()){
ntri = coldetModel->getNumTriangles();
Eigen::Vector3f n, v[3];
int vindex[3];
if (m_highlight){
float red[] = {1,0,0,1};
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, red);
}else{
float gray[] = {0.8,0.8,0.8,1};
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, gray);
}
glBegin(GL_TRIANGLES);
for (int i=0; i<coldetModel->getNumTriangles(); i++){
coldetModel->getTriangle(i, vindex[0], vindex[1], vindex[2]);
for (int j=0; j<3; j++){
coldetModel->getVertex(vindex[j], v[j][0], v[j][1], v[j][2]);
}
n = (v[1]-v[0]).cross(v[2]-v[0]);
n.normalize();
glNormal3fv(n.data());
for (int j=0; j<3; j++){
glVertex3fv(v[j].data());
}
}
glEnd();
}
}
for (size_t i=0; i<sensors.size(); i++){
Sensor *s = sensors[i];
double T[16];
for (int i=0; i<3; i++){
for (int j=0; j<3; j++){
T[i*4+j] = s->localR(j,i);
}
}
T[12] = s->localPos[0]; T[13] = s->localPos[1]; T[14] = s->localPos[2];
T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
glPushMatrix();
glMultMatrixd(T);
GLbody *glb = dynamic_cast<GLbody *>(body);
glb->drawSensor(s);
glPopMatrix();
}
for (size_t i=0; i<lights.size(); i++){
Light *l = lights[i];
int lid = GL_LIGHT0+l->id;
if (!l->on){
glDisable(lid);
continue;
}
double T[16];
for (int j=0; j<3; j++){
for (int k=0; k<3; k++){
T[j*4+k] = l->localR(k,j);
}
}
T[12] = l->localPos[0]; T[13] = l->localPos[1]; T[14] = l->localPos[2];
T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
glPushMatrix();
glMultMatrixd(T);
glEnable(GL_LIGHTING);
glEnable(lid);
GLfloat pos[] = {0,0,0,1};
glLightfv(lid, GL_POSITION, pos);
GLfloat color[] = {l->color[0], l->color[1], l->color[2], 1};
glLightfv(lid, GL_DIFFUSE, color);
if (l->type == POINT){
glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]);
glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]);
glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
}else if(l->type == SPOT){
glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]);
glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]);
glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
glLightf(lid, GL_SPOT_EXPONENT, 20);
glLightf(lid, GL_SPOT_CUTOFF, l->cutOffAngle*180/M_PI);
GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
glLightfv(lid, GL_SPOT_DIRECTION, dir);
}else if(l->type == DIRECTIONAL){
GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
glLightfv(lid, GL_SPOT_DIRECTION, dir);
}
glPopMatrix();
}
if (m_showAxes){
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(1,0,0);
glVertex3f(0,0,0); glVertex3f(0.5, 0, 0);
glColor3f(0,1,0);
glVertex3f(0,0,0); glVertex3f(0, 0.5, 0);
glColor3f(0,0,1);
glVertex3f(0,0,0); glVertex3f(0, 0, 0.5);
glEnd();
glEnable(GL_LIGHTING);
}
if (!m_useAbsTransformToDraw){
hrp::Link *l = child;
while (l){
ntri += ((GLlink *)l)->draw();
l = l->sibling;
}
}
glPopMatrix();
return ntri;
}
