void Robot::drawWing(Pose pose,int midYIndex ,int yIndex, int zIndex) const
{
//glRotatef(0,pose.angles_[yIndex], pose.angles_[zIndex],1);
glRotatef(0,0,430 ,1);
glTranslatef(-WB1/2.0,0,0);
drawCuboid(3,.3,.3);
// joint 2
glPushMatrix();
glTranslatef(-WB1/2.0,0,0);
glRotatef(pose.angles_[midYIndex],0,0,1);
glTranslatef(-WB1/2.0,0,0);
drawCuboid(3,.3,.3);
// joint3
glPushMatrix();
glTranslatef(-WB1/2.0,0,0);
glRotatef(20,0,0,1);
glTranslatef(-WBL/2,0,0);
drawCuboid(WBL,.3,.3);
glPopMatrix();
glTranslatef(-WB1/2.0,0,0);
glRotatef(90,0,0,1);
glTranslatef(-WBL/2,0,0);
drawCuboid(WBL,.2,.2);
glPopMatrix();
glTranslatef(-WB1/2.0,0,0);
glRotatef(90,0,0,1);
glTranslatef(-WBL/2,0,0);
drawCuboid(WBL,.2,.2);
}
void Schema3Layer::draw()
{
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslatef(0.0, 0.0, -10.0);
glRotatef(rotationX, 0.0, 0.0, 0.0);
glRotatef(rotationY, 0.0, 0.0, 0.0);
glRotatef(rotationZ, 0.0, 0.0, 0.0);
// Right
static GLfloat center1[3] = {1.5, 0.0, 0.0};
static GLfloat color1[3] = {0.0, 0.0, 1.0};
static GLfloat length1[3] = {1.0, 0.5, 0.5};
drawCuboid(center1, length1, color1);
// Center
static GLfloat center2[3] = {0.0, 0.0, 0.0};
static GLfloat color2[3] = {0.0, 1.0, 0.0};
static GLfloat length2[3] = {0.5, 0.5, 0.5};
drawCuboid(center2, length2, color2);
// Left
static GLfloat center3[3] = {-1.5, 0.0, 0.0};
static GLfloat color3[3] = { 0.0, 0.0, 1.0};
static GLfloat length3[3] = { 1.0, 0.5, 0.5};
drawCuboid(center3, length3, color3);
glBegin(GL_LINES);
glColor3f(0.0f,0.0f,0.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 1.0f, 0.0f);
glEnd();
}
void Robot::drawArm(Pose pose, int shoulderIndex, int elbowIndex) const
{
//Rotate shouler join
glRotatef(pose.angles_[shoulderIndex],0,0,1); //Rotate WORLDDDD
glTranslatef(0, -UARM_LEN/2.0,0); //translate so we draw the arm rotating WRT shoulder joint
drawCuboid(UARM_WIDTH, UARM_LEN, UARM_WIDTH);
// Move to elbow link point
glTranslatef(0, -UARM_LEN/2.0,0);
glRotatef(pose.angles_[elbowIndex],0,0,1); //Rotate WORLDDDD
glTranslatef(0, -UARM_LEN/2.0,0);
drawCuboid(LARM_WIDTH, LARM_LEN, LARM_WIDTH);
}
void Robot::drawLeg(Pose pose, int hipIndex, int kneeIndex, int ankleIndex) const
{
//Rotate hips
glRotatef(pose.angles_[hipIndex], 0, 0, 1);
//Extend thighs to shift origin
glTranslatef(0, -LEG_LEN/2.0,0);
drawCuboid(LEG_WIDTH,LEG_LEN,LEG_WIDTH);
//shift origin to knee joint
glTranslatef(0, -LEG_LEN/2.0,0);
//rotate knees
glRotatef(pose.angles_[kneeIndex], 0, 0, 1);
//shift knees
glTranslatef(0, -LEG_LEN/2.0,0);
drawCuboid(LEG_WIDTH,LEG_LEN,LEG_WIDTH);
//shift to ankles
glTranslatef(0, -LEG_LEN/2.0,0);
glRotatef(pose.angles_[ankleIndex], 0, 0, 1);
glTranslatef(FEET_LEN/4.0,0,0);
drawCuboid(FEET_LEN,FEET_THICK,FEET_WIDTH);
}
void drawBody(const DYN_Body *body)
{
const double LINEAR_VELOCITY_SCALE_FACTOR = 50;
const double ANGULAR_VELOCITY_SCALE_FACTOR = 50;
// graw velocity and angular momentum
glBegin(GL_LINES);
{
glColor3f(1, 0.5, 0);
glVertex3f(
body->position[0],
body->position[1],
body->position[2]
);
glVertex3f(
body->position[0] + body->velocity[0] * LINEAR_VELOCITY_SCALE_FACTOR,
body->position[1] + body->velocity[1] * LINEAR_VELOCITY_SCALE_FACTOR,
body->position[2] + body->velocity[2] * LINEAR_VELOCITY_SCALE_FACTOR
);
glColor3f(0, 0.5, 1);
glVertex3f(
body->position[0],
body->position[1],
body->position[2]
);
glVertex3f(
body->position[0] + body->angularVelocity[0] * ANGULAR_VELOCITY_SCALE_FACTOR,
body->position[1] + body->angularVelocity[1] * ANGULAR_VELOCITY_SCALE_FACTOR,
body->position[2] + body->angularVelocity[2] * ANGULAR_VELOCITY_SCALE_FACTOR
);
}
glEnd();
if (body->inContact)
{
glColor3f(1, 1, 0);
}
else if (body->colliding)
{
glColor3f(1, 0, 0);
}
else
{
glColor3f(0, 1, 0);
}
switch (body->staticAttributes->shape)
{
case DYN_BS_CUBOID:
{
drawCuboid(body, body->staticAttributes);
}
break;
}
}
void Shapes::update()
{
Geometry::update();
for (unsigned int i=0; i<geom.size(); i++)
{
glNewList(displaylists[i], GL_COMPILE);
if (!drawable(i)) {glEndList(); continue;} ////
//Calibrate colour map (not yet available)
geom[i]->colourCalibrate();
//Set draw state
setState(i);
float scaling = geom[i]->draw->scaling;
//Load constant scaling factors from properties
float dims[3];
dims[0] = geom[i]->draw->props.Float("width", FLT_MIN);
dims[1] = geom[i]->draw->props.Float("height", FLT_MIN);
dims[2] = geom[i]->draw->props.Float("length", FLT_MIN);
int shape = geom[i]->draw->props.Int("shape");
if (scaling <= 0) scaling = 1.0;
for (int v=0; v < geom[i]->count; v++)
{
//Calculate colour
geom[i]->setColour(v);
//Scale the dimensions by variables (dynamic range options? by setting max/min?)
float sdims[3] = {dims[0], dims[1], dims[2]};
if (geom[i]->xWidths.size() > 0) sdims[0] = geom[i]->xWidths[v];
if (geom[i]->yHeights.size() > 0) sdims[1] = geom[i]->yHeights[v]; else sdims[1] = sdims[0];
if (geom[i]->zLengths.size() > 0) sdims[2] = geom[i]->zLengths[v]; else sdims[2] = sdims[1];
//Multiply by constant scaling factors if present
if (dims[0] != FLT_MIN) sdims[0] *= dims[0];
if (dims[1] != FLT_MIN) sdims[1] *= dims[1];
if (dims[2] != FLT_MIN) sdims[2] *= dims[2];
//Apply scaling
sdims[0] *= scaling * scale;
sdims[1] *= scaling * scale;
sdims[2] *= scaling * scale;
float pos[3];
memcpy(pos, geom[i]->vertices[v], 3 * sizeof(float));
//Scale manually (as global scaling is disabled to avoid distorting glyphs)
for (int d=0; d<3; d++)
pos[d] *= view->scale[d];
// Translate to centre position
glPushMatrix();
glTranslatef(pos[0], pos[1], pos[2]);
//Orient view to the alignment vector
if (geom[i]->vectors.size() > 0)
{
Vec3d vec(geom[i]->vectors[v]);
vec *= Vec3d(view->scale); //Scale
// Rotate to orient the cone
//...Want to align our z-axis to point along arrow vector:
// axis of rotation = (z x vec)
// cosine of angle between vector and z-axis = (z . vec) / |z|.|vec| */
vec.normalise();
float rangle = RAD2DEG * vec.angle(Vec3d(0.0, 0.0, 1.0));
//Axis of rotation = vec x [0,0,1] = -vec[1],vec[0],0
glRotatef(rangle, -vec[1], vec[0], 0);
}
//Draw shape
float zpos[3] = {0};
if (shape == 1)
//drawCuboid(min, max, true);
drawCuboid(zpos, sdims[0], sdims[1], sdims[2], true);
else
drawEllipsoid(zpos, sdims[0], sdims[1], sdims[2], 24, NULL);
//Restore model view
glPopMatrix();
}
glEndList();
}
}
void Robot::draw() const {
// Retrieve the current (interpolated) pose of the robot.
Pose pose = getPose();
// You can set the robot to be whatever color you like.
// By default it is white.
glPushMatrix();
glMatrixMode(GL_MODELVIEW);
//BODY
glColor4d(1, 1, 1, 1);
glTranslatef(pose.x_, pose.y_,pose.z_);
glRotatef(pose.angles_[BODY_ANGLE], 0, 0, 1);
drawCuboid(BODY_X,BODY_LEN,BODY_Z);
//HEAD
glPushMatrix();
glTranslatef(0,BODY_LEN/2,0);
glRotatef(pose.angles_[HEAD_ANGLE], 0, 0, 1);
glTranslatef(0,.3,0);
drawCuboid(.6,.6,.6);
glPopMatrix();
// ARMS!!
//Right
glPushMatrix();
glTranslatef(0, BODY_LEN/2.0, BODY_Z/2.0 + UARM_WIDTH/2.0); //Move to shoulder joint from center of body
drawArm(pose, RIGHT_SHOULDER, RIGHT_ELBOW);
glPopMatrix();
//Left
glPushMatrix();
glTranslatef(0, BODY_LEN/2.0, - (BODY_Z/2.0 + UARM_WIDTH/2.0)); //Move to shoulder joint from center of body
drawArm(pose, LEFT_SHOULDER, LEFT_ELBOW);
glPopMatrix();
//LEGS
//RIGHT
glPushMatrix();
glTranslatef(0, -1, .25);
drawLeg(pose, RIGHT_HIP, RIGHT_KNEE, RIGHT_ANKLE);
glPopMatrix();
//LEFT
glPushMatrix();
glTranslatef(0, -1, -.25);
drawLeg(pose, LEFT_HIP, LEFT_KNEE, LEFT_ANKLE);
glPopMatrix();
//WINGS
//RIGHT
glPushMatrix();
glTranslatef(-BODY_X/2.0,BODY_LEN/2.0 -.2,BODY_Z/2.0 - .2);
drawWing(pose, RIGHT_WING_MID_Y, RIGHT_WING_Y, RIGHT_WING_Z);
glPopMatrix();
//LEFT
glPushMatrix();
glTranslatef(-BODY_X/2.0,BODY_LEN/2.0 -.2, -BODY_Z/2.0 + .2);
drawWing(pose, LEFT_WING_MID_Y, LEFT_WING_Y, LEFT_WING_Z);
glPopMatrix();
//Body matrix
glPopMatrix();
}
