void RobotTestBackend::RenderWorld()
{
ViewRobot& viewRobot = world->robotViews[0];
//WorldViewProgram::RenderWorld();
glDisable(GL_LIGHTING);
drawCoords(0.1);
glEnable(GL_LIGHTING);
for(size_t i=0;i<world->terrains.size();i++)
world->terrains[i]->DrawGL();
for(size_t i=0;i<world->rigidObjects.size();i++)
world->rigidObjects[i]->DrawGL();
if(draw_sensors) {
if(robotSensors.sensors.empty()) {
robotSensors.MakeDefault(robot);
}
for(size_t i=0;i<robotSensors.sensors.size();i++) {
vector<double> measurements;
if(0 == strcmp(robotSensors.sensors[i]->Type(),"CameraSensor")) {
robotSensors.sensors[i]->SimulateKinematic(*robot,*world);
robotSensors.sensors[i]->GetMeasurements(measurements);
}
robotSensors.sensors[i]->DrawGL(*robot,measurements);
}
}
if(draw_geom) {
//set the robot colors
GLColor highlight(1,1,0);
GLColor driven(1,0.5,0);
GLColor colliding(1,0,0);
GLColor blue(0,0,1);
viewRobot.RestoreAppearance();
viewRobot.PushAppearance();
for(size_t i=0;i<robot->links.size();i++) {
if(self_colliding[i]) viewRobot.SetColor(i,colliding);
if((int)i == cur_link)
viewRobot.SetColor(i,highlight);
else if(cur_driver >= 0 && cur_driver < (int)robot->drivers.size() &&
robot->DoesDriverAffect(cur_driver,i))
viewRobot.SetColor(i,driven);
if(draw_self_collision_tests) {
//draw a little blue
if(robot->selfCollisions(i,cur_link) || robot->selfCollisions(cur_link,i) ) {
GLDraw::GeometryAppearance &app = viewRobot.Appearance(i);
app.ModulateColor(blue,0.5);
}
}
}
//this will set the hover colors
allWidgets.DrawGL(viewport);
//viewRobot.Draw();
viewRobot.PopAppearance();
}
else {
for(size_t i=0;i<robotWidgets.size();i++)
robotWidgets[i].linkPoser.draw = 0;
allWidgets.DrawGL(viewport);
for(size_t i=0;i<robotWidgets.size();i++)
robotWidgets[i].linkPoser.draw = 1;
}
if(draw_com) {
viewRobot.DrawCenterOfMass();
for(size_t i=0;i<robot->links.size();i++)
viewRobot.DrawLinkCenterOfMass(i,0.01);
}
if(draw_frame) {
viewRobot.DrawLinkFrames();
glDisable(GL_DEPTH_TEST);
glPushMatrix();
glMultMatrix((Matrix4)robot->links[cur_link].T_World);
drawCoords(0.2);
glPopMatrix();
glEnable(GL_DEPTH_TEST);
}
if(draw_self_collision_tests) {
glDisable(GL_LIGHTING);
glLineWidth(2.0);
glColor3f(1,0,0);
glBegin(GL_LINES);
for(size_t i=0;i<robot->links.size();i++) {
Vector3 comi = robot->links[i].T_World*robot->links[i].com;
for(size_t j=0;j<robot->links.size();j++) {
if(robot->selfCollisions(i,j)) {
Vector3 comj = robot->links[j].T_World*robot->links[j].com;
glVertex3v(comi);
glVertex3v(comj);
}
}
}
glEnd();
}
if(draw_bbs) {
for(size_t j=0;j<robot->geometry.size();j++) {
if(robot->IsGeometryEmpty(j)) continue;
Box3D bbox = robot->geometry[j]->GetBB();
Matrix4 basis;
bbox.getBasis(basis);
glColor3f(1,0,0);
drawOrientedWireBox(bbox.dims.x,bbox.dims.y,bbox.dims.z,basis);
}
}
}
virtual void RenderWorld()
{
if(env) {
glEnable(GL_LIGHTING);
viewEnv.Draw(env);
glDisable(GL_LIGHTING);
}
//drawCoords(0.1);
//viewRobot.DrawJointCoords();
if(draw_geom) {
//set the robot colors
GLColor hover(0.75,0.75,0);
GLColor highlight(1,1,0);
GLColor driven(1,0.5,0);
GLColor colliding(1,0,0);
viewRobot.SetGrey();
for(size_t i=0;i<robot->links.size();i++) {
if(self_colliding[i]) viewRobot.SetColor(i,colliding);
if((int)i == hoverLink)
viewRobot.SetColor(i,hover);
else if((int)i == cur_link)
viewRobot.SetColor(i,highlight);
else if(cur_driver >= 0 && cur_driver < (int)robot->drivers.size() &&
robot->DoesDriverAffect(cur_driver,i))
viewRobot.SetColor(i,driven);
}
viewRobot.Draw();
}
if(draw_com) {
viewRobot.DrawCenterOfMass();
for(size_t i=0;i<robot->links.size();i++)
viewRobot.DrawLinkCenterOfMass(i,0.01);
}
if(draw_frame) {
viewRobot.DrawLinkFrames();
glDisable(GL_DEPTH_TEST);
glPushMatrix();
glMultMatrix((Matrix4)robot->links[cur_link].T_World);
drawCoords(0.2);
glPopMatrix();
glEnable(GL_DEPTH_TEST);
}
if(draw_self_collision_tests) {
glDisable(GL_LIGHTING);
glLineWidth(2.0);
glColor3f(1,0,0);
glBegin(GL_LINES);
for(size_t i=0;i<robot->links.size();i++) {
Vector3 comi = robot->links[i].T_World*robot->links[i].com;
for(size_t j=0;j<robot->links.size();j++) {
if(robot->selfCollisions(i,j)) {
Vector3 comj = robot->links[j].T_World*robot->links[j].com;
glVertex3v(comi);
glVertex3v(comj);
}
}
}
glEnd();
}
if(draw_bbs) {
for(size_t j=0;j<robot->geometry.size();j++) {
if(robot->geometry[j].Empty()) continue;
Box3D bbox = robot->geometry[j].GetBB();
Matrix4 basis;
bbox.getBasis(basis);
glColor3f(1,0,0);
drawOrientedWireBox(bbox.dims.x,bbox.dims.y,bbox.dims.z,basis);
}
}
if(!poseGoals.empty()) {
glPolygonOffset(0,-1000);
glEnable(GL_POLYGON_OFFSET_FILL);
for(size_t i=0;i<poseGoals.size();i++) {
Vector3 curpos = robot->links[poseGoals[i].link].T_World*poseGoals[i].localPosition;
Vector3 despos = poseGoals[i].endPosition;
glDisable(GL_LIGHTING);
glColor3f(1,0,0);
glLineWidth(5.0);
glBegin(GL_LINES);
glVertex3v(curpos);
glVertex3v(despos);
glEnd();
glLineWidth(1.0);
poseWidgets[i].DrawGL(viewport);
float color2[4] = {1,0.5,0,1};
glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,color2);
glPushMatrix();
if(poseGoals[i].rotConstraint == IKGoal::RotFixed) {
RigidTransform T;
poseGoals[i].GetFixedGoalTransform(T);
glMultMatrix(Matrix4(T));
drawBox(0.04,0.04,0.04);
}
else {
glTranslate(despos);
drawSphere(0.02,16,8);
}
glPopMatrix();
}
glDisable(GL_POLYGON_OFFSET_FILL);
}
}