void SDLwindow::draw()
{
if (scene->getDefaultCamera() == scene->getCamera()){
hrp::BodyPtr target = scene->targetObject();
double yaw = pan;
double x,y,z;
if (target){
GLlink *root = (GLlink *)target->rootLink();
root->getPosition(x,y,z);
hrp::Matrix33 R;
root->getRotation(R);
hrp::Vector3 rpy = hrp::rpyFromRot(R);
yaw += rpy[2];
}else{
x = xCenter; y = yCenter; z = zCenter;
}
double xEye = x + radius*cos(tilt)*cos(yaw);
double yEye = y + radius*cos(tilt)*sin(yaw);
double zEye = z + radius*sin(tilt);
scene->getCamera()->setViewTarget(x,y,z);
scene->getCamera()->setViewPoint(xEye, yEye, zEye);
}
scene->setView();
glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);
scene->draw();
}
void GLscene::updateScene()
{
if (m_log->index()<0) return;
#ifdef USE_COLLISION_STATE
LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
= (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
if (co.angle.length() == glbody->numJoints()){
for (int i=0; i<glbody->numJoints(); i++){
GLlink *j = (GLlink *)glbody->joint(i);
if (j){
j->setQ(co.angle[i]);
}
}
}
#else
LogManager<TimedPosture> *lm
= (LogManager<TimedPosture> *)m_log;
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
TimedPosture &ts = lm->state();
if (ts.posture.size() == glbody->numJoints()){
for (int i=0; i<glbody->numJoints(); i++){
GLlink *j = (GLlink *)glbody->joint(i);
if (j){
j->setQ(ts.posture[i]);
}
}
}
#endif
}
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;
}
}
}
