void GLscene::updateScene()
{
if (m_log->index()<0) return;
LogManager<OpenHRP::WorldState> *lm
= (LogManager<OpenHRP::WorldState> *)m_log;
OpenHRP::WorldState &state = lm->state();
for (unsigned int i=0; i<state.characterPositions.length(); i++){
const CharacterPosition& cpos = state.characterPositions[i];
std::string cname(cpos.characterName);
GLbody *glbody = dynamic_cast<GLbody *>(body(cname).get());
if (!glbody) {
//std::cerr << "can't find a body named " << cname << std::endl;
continue;
}
for (unsigned int j=0; j<cpos.linkPositions.length(); j++){
const LinkPosition &lp = cpos.linkPositions[j];
double T[] = {lp.R[0], lp.R[3], lp.R[6],0,
lp.R[1], lp.R[4], lp.R[7],0,
lp.R[2], lp.R[5], lp.R[8],0,
lp.p[0], lp.p[1], lp.p[2],1};
#if 0
for (int i=0; i<4; i++){
for (int j=0; j<4; j++){
printf("%6.3f ", T[i*4+j]);
}
printf("\n");
}
printf("\n");
#endif
((GLlink *)glbody->link(j))->setAbsTransform(T);
}
}
}
void GLscene::showStatus()
{
char buf[256];
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
int width = m_width - 220;
#define HEIGHT_STEP 12
int height = m_height-HEIGHT_STEP;
int x = width;
for (int i=0; i<glbody->numLinks(); i++){
hrp::Link *l = glbody->link(i);
if (l){
sprintf(buf, "%13s %4d tris",
l->name.c_str(),
l->coldetModel->getNumTriangles());
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
}
}
if (m_log->index()<0) return;
LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
= (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
height -= HEIGHT_STEP;
x = width - 34;
sprintf(buf, "Number of pair %8d", co.lines.length());
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Calc Time [msec] %8.3f", co.computation_time);
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Recover Time[msec] %8.3f", co.recover_time);
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Safe Posture %8s", co.safe_posture?"true":"false");
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Loop for check %8d", co.loop_for_check);
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
}
void GLscene::updateScene()
{
if (m_log->index()<0) return;
LogManager<TimedRobotState> *lm
= (LogManager<TimedRobotState> *)m_log;
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
OpenHRP::StateHolderService::Command &com = lm->state().command;
if (com.baseTransform.length() == 12){
double *tform = com.baseTransform.get_buffer();
glbody->setPosition(tform);
glbody->setRotation(tform+3);
hrp::Link *root = glbody->rootLink();
root->p << tform[0], tform[1], tform[2];
root->R << tform[3], tform[4], tform[5],
tform[6], tform[7], tform[8],
tform[9], tform[10], tform[11];
}
if (com.jointRefs.length() == glbody->numJoints()){
for (int i=0; i<glbody->numJoints(); i++){
GLlink *j = (GLlink *)glbody->joint(i);
if (j){
j->q = com.jointRefs[i];
j->setQ(com.jointRefs[i]);
}
}
}
glbody->calcForwardKinematics();
glbody->updateLinkColdetModelPositions();
for (int i=0; i<glbody->numLinks(); i++){
((GLlink *)glbody->link(i))->highlight(false);
}
for (size_t i=0; i<m_pairs.size(); i++){
if (m_pairs[i]->checkCollision()){
((GLlink *)m_pairs[i]->link(0))->highlight(true);
((GLlink *)m_pairs[i]->link(1))->highlight(true);
std::cout << m_pairs[i]->link(0)->name << "<->" << m_pairs[i]->link(1)->name << std::endl;
}
}
}
void GLscene::showStatus()
{
if (m_log->index()<0) return;
LogManager<SceneState> *lm
= (LogManager<SceneState> *)m_log;
SceneState &state = lm->state();
if (m_showingStatus){
GLbody *glbody = NULL;
BodyState *bstate = NULL;
for (unsigned int i=0; i<numBodies(); i++){
if (body(i)->numJoints()){
glbody = dynamic_cast<GLbody *>(body(i).get());
bstate = &state.bodyStates[i];
break;
}
}
if (!glbody) return;
#define HEIGHT_STEP 12
int width = m_width - 350;
int height = m_height-HEIGHT_STEP;
char buf[256];
double q[glbody->numJoints()];
for (int i=0; i<glbody->numLinks(); i++){
Link* l = glbody->link(i);
if (l->jointId >= 0) q[l->jointId] = bstate->q[i];
}
for (int i=0; i<glbody->numJoints(); i++){
GLlink *l = (GLlink *)glbody->joint(i);
if (l){
sprintf(buf, "%2d %15s %8.3f", i, l->name.c_str(),
q[i]*180/M_PI);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (bstate->acc.size()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("acc:");
for (unsigned int i=0; i<bstate->acc.size(); i++){
sprintf(buf, " %8.4f %8.4f %8.4f",
bstate->acc[i][0], bstate->acc[i][1], bstate->acc[i][2]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (bstate->rate.size()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("rate:");
for (unsigned int i=0; i<bstate->rate.size(); i++){
sprintf(buf, " %8.4f %8.4f %8.4f",
bstate->rate[i][0], bstate->rate[i][1], bstate->rate[i][2]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (bstate->force.size()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("force/torque:");
for (unsigned int i=0; i<bstate->force.size(); i++){
sprintf(buf, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
bstate->force[i][0],
bstate->force[i][1],
bstate->force[i][2],
bstate->force[i][3],
bstate->force[i][4],
bstate->force[i][5]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glColor4f(0.0,0.0,0.0, 0.5);
if (m_showSlider){
glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
}else{
glRectf(width,0,m_width,m_height);
}
glDisable(GL_BLEND);
}
}
