void RenderPhyFixed::render(const PhyFixedShared* sharedData, GLContextAndData* contextAndData)
{
// Drawing the joint
const wVector end1 = (sharedData->localMatrixParent.inverse() * sharedData->globalMatrixParent).w_pos;
const wVector end2 = (sharedData->localMatrixChild.inverse() * sharedData->globalMatrixChild).w_pos;
drawJoint(sharedData, contextAndData, sharedData->globalMatrixParent.w_pos, end1, end2);
}
void GLWidget::drawNeck(const float NECK_X_SCALE, const float NECK_Y_SCALE,
const float BEAK_UP_X_SCALE, const float BEAK_UP_Y_SCALE,
const float BEAK_DOWN_X_SCALE, const float BEAK_DOWN_Y_SCALE,
const float BODY_Y_SCALE)
{
transformStack().pushMatrix();
// position neck and joint together
transformStack().translate(-2.5, BODY_Y_SCALE/2.5);
transformStack().pushMatrix();
transformStack().rotateInDegrees(neck_joint_angle);
// Draw beak
drawBeak(NECK_X_SCALE,BEAK_DOWN_X_SCALE,BEAK_DOWN_Y_SCALE,
BEAK_UP_X_SCALE,BEAK_UP_Y_SCALE);
transformStack().scale(NECK_X_SCALE,NECK_Y_SCALE); // 50,40
transformStack().translate(0.0, 0.4);
m_gl_state.setColor(0.0, 1.0, 1.0);
m_unit_neck.draw();
transformStack().popMatrix();
transformStack().pushMatrix();
drawJoint(1.0, NECK_Y_SCALE/5.0);
transformStack().popMatrix();
transformStack().popMatrix();
}
void app::drawJoint_doEffect(Joint* j, ofPoint multiplier, ofPoint constant, Bone * b, Skeleton * s)
{
//Draw
drawJoint(j, multiplier, constant);
//Do this level's effect as seperate function
doJointEffect(j, multiplier, constant,b,s);
}
void renderModels()
{
drawAxes();
drawXYGrid(10);
V = computeVfromM();
// TRACE("M = " << M);
// TRACE("V = " << V);
// testComputation();
glEnable(GL_LIGHTING);
drawCabin();
drawJoint(O);
drawArm(O, V);
drawJoint(V);
drawArm(V, M);
drawJoint(M);
glDisable(GL_LIGHTING);
}
void GLWidget::drawLeftFoot(const float LEG_X_SCALE, const float LEG_Y_SCALE,
const float FOOT_X_SCALE, const float FOOT_Y_SCALE)
{
// position left foot and joint together
transformStack().translate(LEG_X_SCALE/2-FOOT_X_SCALE/6, -LEG_Y_SCALE/2+FOOT_X_SCALE/2);
// left foot
transformStack().pushMatrix();
transformStack().rotateInDegrees(leftfoot_joint_angle);
transformStack().scale(FOOT_X_SCALE,FOOT_Y_SCALE);
transformStack().translate(0.0, -0.5); // Move down so that the centre of rotation is at top
m_gl_state.setColor(0.0, 1.0, 0.0);
m_unit_foot.draw();
transformStack().popMatrix();
// left foot joint
transformStack().pushMatrix();
drawJoint(-4.0, 0.0);
transformStack().popMatrix();
}
void GLWidget::drawLeftArm(const float ARM_X_SCALE, const float ARM_Y_SCALE)
{
transformStack().pushMatrix();
// position left arm and joint together
transformStack().translate(0.0, 30.0);
transformStack().pushMatrix();
transformStack().rotateInDegrees(leftarm_joint_angle);
transformStack().scale(ARM_X_SCALE,ARM_Y_SCALE);
transformStack().translate(0.0, -0.4);
m_gl_state.setColor(0.0, 1.0, 0.0);
m_unit_arm.draw();
transformStack().popMatrix();
transformStack().pushMatrix();
drawJoint(0.0, 4.0);
transformStack().popMatrix();
transformStack().popMatrix();
}
void GLWidget::drawLeftLegFoot(const float LEG_X_SCALE, const float LEG_Y_SCALE,
const float LEG_X_INV_SCALE, const float LEG_Y_INV_SCALE,
const float FOOT_X_SCALE, const float FOOT_Y_SCALE)
{
transformStack().translate(-30.0,-50.0);
transformStack().rotateInDegrees(leftleg_joint_angle);
transformStack().scale(LEG_X_SCALE,LEG_Y_SCALE);
transformStack().translate(0.0, -0.5);
m_gl_state.setColor(1.0, 0.0, 0.0);
m_unit_square.draw();
// since foot and joint as children of leg are already drawn,
// to scale leg, need to inverse scale children of leg and scale everything back
transformStack().scale(LEG_X_INV_SCALE,LEG_Y_INV_SCALE);
transformStack().pushMatrix();
drawJoint(0.0, LEG_Y_SCALE/2-4.0);
transformStack().popMatrix();
// Start drawing left foot and joint
transformStack().pushMatrix();
drawLeftFoot(LEG_X_SCALE,LEG_Y_SCALE,FOOT_X_SCALE,FOOT_Y_SCALE);
transformStack().popMatrix();
}
/// <summary>
/// Draw skeleton.
/// </summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth stream image is streched to fit</param>
void UKinect::drawSkeleton(const NUI_SKELETON_DATA& skeletonData) {
// Torso
drawBone(skeletonData, NUI_SKELETON_POSITION_HEAD, NUI_SKELETON_POSITION_SHOULDER_CENTER);
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SPINE);
drawBone(skeletonData, NUI_SKELETON_POSITION_SPINE, NUI_SKELETON_POSITION_HIP_CENTER);
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_RIGHT);
// Left arm
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_LEFT, NUI_SKELETON_POSITION_ELBOW_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ELBOW_LEFT, NUI_SKELETON_POSITION_WRIST_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_WRIST_LEFT, NUI_SKELETON_POSITION_HAND_LEFT);
// Right arm
drawBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_RIGHT, NUI_SKELETON_POSITION_ELBOW_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ELBOW_RIGHT, NUI_SKELETON_POSITION_WRIST_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_WRIST_RIGHT, NUI_SKELETON_POSITION_HAND_RIGHT);
// Left leg
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_LEFT, NUI_SKELETON_POSITION_KNEE_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_KNEE_LEFT, NUI_SKELETON_POSITION_ANKLE_LEFT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ANKLE_LEFT, NUI_SKELETON_POSITION_FOOT_LEFT);
// Right leg
drawBone(skeletonData, NUI_SKELETON_POSITION_HIP_RIGHT, NUI_SKELETON_POSITION_KNEE_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_KNEE_RIGHT, NUI_SKELETON_POSITION_ANKLE_RIGHT);
drawBone(skeletonData, NUI_SKELETON_POSITION_ANKLE_RIGHT, NUI_SKELETON_POSITION_FOOT_RIGHT);
// Draw joints
for (int i = 0; i < NUI_SKELETON_POSITION_COUNT; i++) {
drawJoint(skeletonData, (NUI_SKELETON_POSITION_INDEX)i);
}
}
void GLCanvas::renderSkeleton()
{
Vector3 lookAt = _cameraPosition + _cameraFront;
gluLookAt(_cameraPosition.x(), _cameraPosition.y(), _cameraPosition.z(),
lookAt.x() , lookAt.y() , lookAt.z(),
_cameraUp.x() , _cameraUp.y() , _cameraUp.z());
if (_style & DRAW_GRID)
{
drawGrid();
}
if (_skeleton == nullptr)
{
return;
}
for (auto it = _skeleton->beginBones(); it != _skeleton->endBones(); ++it)
{
glPushMatrix();
Bone bone = *(it->second);
int boneId = bone.getId();
// don't draw any bone without valid id
if (boneId < 0)
{
continue;
}
// get the bone id as color for SELECTION_MODE
GLubyte boneIdColor[4] = {GLubyte((boneId >> 8) & 255), GLubyte((boneId >> 8) & 255), GLubyte(boneId & 255), 255};
const GLubyte* boneColor1 = boneStandardColor1;
const GLubyte* boneColor2 = boneStandardColor2;
if (_style & SELECTION_MODE)
{
boneColor1 = boneIdColor;
boneColor2 = boneIdColor;
}
else if (_style & HIGHLIGHT_SELECTED_BONE && bone.getId() == _skeleton->getSelectedBoneId())
{
boneColor1 = boneHighlightedColor1;
boneColor2 = boneHighlightedColor2;
}
Vector3 startPos = bone.getStartPos();
glTranslatef(startPos.x(), startPos.y(), startPos.z());
float length = bone.getLength();
Vector3 dir = bone.getDirection();
Vector3 up = bone.getUpDirection();
Vector3 right = bone.getRightDirection();
Vector3 endPos = dir*length;
startPos = Vector3(0, 0, 0);
float length_10 = length * 0.1f;
Vector3 endPos_10 = endPos * 0.1f;
Vector3 upPoint = up*length_10 + endPos_10;
Vector3 downPoint = - up*length_10 + endPos_10;
Vector3 rightPoint = right*length_10 + endPos_10;
Vector3 leftPoint = - right*length_10 + endPos_10;
if (!(_style & SELECTION_MODE))
{
//set point size to 10 pixels
glPointSize(10.0f);
glColor4ubv(pointColor);
// TODO(JK#9#): maybe don't draw points for bones (or add render style flag)
glBegin(GL_POINTS);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glEnd();
}
// set line width
glLineWidth(_lineWidth);
// draw local coordinate system
if (_style & DRAW_LOCAL_COORDINATE_SYSTEM)
{
glBegin(GL_LINES);
glColor4ubv(red);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(endPos.x() + dir.x()*0.1f, endPos.y() + dir.y()*0.1f, endPos.z() + dir.z()*0.1f);
glColor4ubv(green);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(endPos.x() + up.x()*0.1f, endPos.y() + up.y()*0.1f, endPos.z() + up.z()*0.1f);
glColor4ubv(blue);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(endPos.x() + right.x()*0.1f, endPos.y() + right.y()*0.1f, endPos.z() + right.z()*0.1f);
glEnd();
}
if (_style & DRAW_ROTATION_AXIS)
{
Vector3 rotAxis = bone.getRelOrientation().getRotationAxis();
glBegin(GL_LINES);
glColor4ubv(yellow);
glVertex3f(-rotAxis.x()*0.2f, -rotAxis.y()*0.2f, -rotAxis.z()*0.2f);
glVertex3f(rotAxis.x()*0.2f, rotAxis.y()*0.2f, rotAxis.z()*0.2f);
glEnd();
}
// draw bone
glPolygonMode(GL_FRONT, GL_FILL);
glColor4ubv(boneColor1);
glBegin(GL_TRIANGLE_FAN);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
glColor4ubv(boneColor2);
glBegin(GL_TRIANGLE_FAN);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
if (!(_style & SELECTION_MODE))
{
// draw black mesh lines around bones
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4ubv(black);
glBegin(GL_TRIANGLE_FAN);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
glBegin(GL_TRIANGLE_FAN);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
// draw labels
if (_style & DRAW_LABEL)
{
// glDisable(GL_DEPTH_TEST);
GLImage* image = _labels.find(boneId)->second;
image->setPosition(0.5f * bone.getLength() * dir - 0.06f * bone.getLength() * _cameraFront);
image->render();
// glEnable(GL_DEPTH_TEST);
}
}
// reset line width
glLineWidth(1.0f);
if (_style & DRAW_SPIN_ARROWS && bone.getId() == _skeleton->getSelectedBoneId())
{
drawSpinArrows(endPos - 0.2 * bone.getLength() * dir, dir, up, right);
}
glPopMatrix();
// append trace point and draw trace
if (_traceLength > 0)
{
Vector3 globalEndPos = bone.getEndPos();
std::map<int, std::vector<Vector3> >::iterator traceIt = _boneIdsWithTracePoints.find(boneId);
if (traceIt != _boneIdsWithTracePoints.end())
{
if (traceIt->second.size() < _traceLength)
{
traceIt->second.push_back(globalEndPos);
}
else
{
traceIt->second[_tracePos] = globalEndPos;
}
glBegin(GL_LINES);
glColor4ubv(yellow);
for (size_t i = 0; i < traceIt->second.size() - 1; ++i)
{
if (i != _tracePos)
{
glVertex3f(traceIt->second[i].x(), traceIt->second[i].y(), traceIt->second[i].z());
glVertex3f(traceIt->second[i+1].x(), traceIt->second[i+1].y(), traceIt->second[i+1].z());
}
}
// draw gab between end and beginning of the vector data
if (traceIt->second.size() > 1 && _tracePos != traceIt->second.size() - 1)
{
glVertex3f(traceIt->second.back().x(), traceIt->second.back().y(), traceIt->second.back().z());
glVertex3f(traceIt->second[0].x(), traceIt->second[0].y(), traceIt->second[0].z());
}
glEnd();
}
}
}
// draw joints (after everything else, as they are transparent and need everything else be rendered)
if (_style & DRAW_JOINT_CONSTRAINTS)
{
for (auto it = _skeleton->beginBones(); it != _skeleton->endBones(); ++it)
{
glPushMatrix();
Bone bone = *(it->second);
Vector3 startPos = bone.getStartPos();
glTranslatef(startPos.x(), startPos.y(), startPos.z());
JointConstraint constraint = bone.getJointConstraint();
drawJoint(constraint);
glPopMatrix();
}
}
// update trace position
if (++_tracePos >= _traceLength)
{
_tracePos = 0;
}
if (!(_style & SELECTION_MODE) && _style & DRAW_AABB)
{
drawAABB(_skeleton->getAABB());
}
}
void GLCanvas::drawSpinArrows(Vector3 pos, Vector3 dir, Vector3 up, Vector3 right) const
{
float length = 0.03f;
float offset = 0.05f;
GLubyte red[4] = {255, 0, 0, 255};
GLubyte green[4] = {0, 255, 0, 255};
GLubyte blue[4] = {0, 0, 255, 255};
GLubyte black[4] = {0, 0, 0, 255};
// set arroe ids to next available free ids
int idArrowDir = _skeleton->getNextFreeId();
int idArrowUp = _skeleton->getNextFreeId() + 1;
int idArrowRight = _skeleton->getNextFreeId() + 2;
GLubyte idArrowDirColor[4] = {GLubyte((idArrowDir >> 8) & 255), GLubyte((idArrowDir >> 8) & 255), GLubyte(idArrowDir & 255), 255};
GLubyte idArrowUpColor[4] = {GLubyte((idArrowUp >> 8) & 255), GLubyte((idArrowUp >> 8) & 255), GLubyte(idArrowUp & 255), 255};
GLubyte idArrowRightColor[4] = {GLubyte((idArrowRight >> 8) & 255), GLubyte((idArrowRight >> 8) & 255), GLubyte(idArrowRight & 255), 255};
const GLubyte* arrowDirColor = red;
const GLubyte* arrowUpColor = green;
const GLubyte* arrowRightColor = blue;
// base points of the arrow pyramids
Vector3 basePointUp = pos + up * offset;
Vector3 basePointRight = pos + right * offset;
Vector3 basePointDown = pos - up * offset;
Vector3 basePointLeft = pos - right * offset;
dir *= length;
up *= length;
right *= length;
Vector3 dirHalf = dir * 0.5;
Vector3 rightHalf = right * 0.5;
// base vertices of the circle like arrow
Vector3 upperCircle[4];
upperCircle[0] = basePointDown;
upperCircle[1] = basePointDown - right;
upperCircle[2] = basePointLeft - up;
upperCircle[3] = basePointLeft;
Vector3 lowerCircle[4];
lowerCircle[0] = basePointDown - up;
lowerCircle[1] = basePointDown - right*1.4 - up;
lowerCircle[2] = basePointLeft - right - up*1.4;
lowerCircle[3] = basePointLeft - right;
// the arrow rendering is done twice, one iteration for filling color, one for having black corner lines
int numIterations = 2;
if (_style & SELECTION_MODE)
{
// do not draw the corner lines in selection mode
numIterations = 1;
// draw the arrows with their id color
arrowDirColor = idArrowDirColor;
arrowUpColor = idArrowUpColor;
arrowRightColor = idArrowRightColor;
}
// draw vertices twice, one run with filling and one with black lines
for (int j = 0; j < numIterations; ++j)
{
if (j == 0)
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}
else
{
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4f(0.0, 0.0, 0.0, 1.0);
arrowDirColor = arrowUpColor = arrowRightColor = black;
}
if (j == 0)
{
glColor4ubv(arrowUpColor);
}
glColor4ubv(arrowUpColor);
// arrow pointing upwards
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointUp.x() + up.x()*2.0, basePointUp.y() + up.y()*2.0, basePointUp.z() + up.z()*2.0);
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glVertex3f(basePointUp.x() + right.x(), basePointUp.y() + right.y(), basePointUp.z() + right.z());
glVertex3f(basePointUp.x() - dir.x(), basePointUp.y() - dir.y(), basePointUp.z() - dir.z());
glVertex3f(basePointUp.x() - right.x(), basePointUp.y() - right.y(), basePointUp.z() - right.z());
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glEnd();
// arrow pointing downwards
/* glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointDown.x() - up.x(), basePointDown.y() - up.y(), basePointDown.z() - up.z());
glVertex3f(basePointDown.x() + dir.x(), basePointDown.y() + dir.y(), basePointDown.z() + dir.z());
glVertex3f(basePointDown.x() + right.x(), basePointDown.y() + right.y(), basePointDown.z() + right.z());
glVertex3f(basePointDown.x() - dir.x(), basePointDown.y() - dir.y(), basePointDown.z() - dir.z());
glVertex3f(basePointDown.x() - right.x(), basePointDown.y() - right.y(), basePointDown.z() - right.z());
glVertex3f(basePointDown.x() + dir.x(), basePointDown.y() + dir.y(), basePointDown.z() + dir.z());
glEnd();
*/
if (j == 0)
{
glColor4f(0.0, 0.0, 1.0, 1.0);
}
glColor4ubv(arrowRightColor);
// arrow pointing to the right
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointRight.x() + right.x()*2.0, basePointRight.y() + right.y()*2.0, basePointRight.z() + right.z()*2.0);
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glVertex3f(basePointRight.x() - up.x(), basePointRight.y() - up.y(), basePointRight.z() - up.z());
glVertex3f(basePointRight.x() - dir.x(), basePointRight.y() - dir.y(), basePointRight.z() - dir.z());
glVertex3f(basePointRight.x() + up.x(), basePointRight.y() + up.y(), basePointRight.z() + up.z());
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glEnd();
// arrow pointing to the left
/* glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointLeft.x() - right.x(), basePointLeft.y() - right.y(), basePointLeft.z() - right.z());
glVertex3f(basePointLeft.x() + dir.x(), basePointLeft.y() + dir.y(), basePointLeft.z() + dir.z());
glVertex3f(basePointLeft.x() - up.x(), basePointLeft.y() - up.y(), basePointLeft.z() - up.z());
glVertex3f(basePointLeft.x() - dir.x(), basePointLeft.y() - dir.y(), basePointLeft.z() - dir.z());
glVertex3f(basePointLeft.x() + up.x(), basePointLeft.y() + up.y(), basePointLeft.z() + up.z());
glVertex3f(basePointLeft.x() + dir.x(), basePointLeft.y() + dir.y(), basePointLeft.z() + dir.z());
glEnd();
*/
if (j == 0)
{
glColor4f(1.0, 0.0, 0.0, 1.0);
}
glColor4ubv(arrowDirColor);
glBegin(GL_TRIANGLE_FAN);
// top of arrow
glVertex3f(basePointLeft.x() - rightHalf.x() + up.x()*2.0, basePointLeft.y() - rightHalf.y() + up.y()*2.0, basePointLeft.z() - rightHalf.z() + up.z()*2.0);
glVertex3f(basePointLeft.x() + rightHalf.x(), basePointLeft.y() + rightHalf.y(), basePointLeft.z() + rightHalf.z());
glVertex3f(basePointLeft.x() - rightHalf.x() + dir.x(), basePointLeft.y() - rightHalf.y() + dir.y(), basePointLeft.z() - rightHalf.z() + dir.z());
glVertex3f(basePointLeft.x() - right.x() - rightHalf.x(), basePointLeft.y() - right.y() - rightHalf.y(), basePointLeft.z() - right.z() - rightHalf.z());
glVertex3f(basePointLeft.x() - rightHalf.x() - dir.x(), basePointLeft.y() - rightHalf.y() - dir.y(), basePointLeft.z() - rightHalf.z() - dir.z());
glVertex3f(basePointLeft.x() + rightHalf.x(), basePointLeft.y() + rightHalf.y(), basePointLeft.z() + rightHalf.z());
glEnd();
// draw arrows base
glBegin(GL_QUAD_STRIP);
for (int i = 0; i < 4; ++i)
{
glVertex3f(upperCircle[i].x() + dirHalf.x(), upperCircle[i].y() + dirHalf.y(), upperCircle[i].z() + dirHalf.z());
glVertex3f(upperCircle[i].x() - dirHalf.x(), upperCircle[i].y() - dirHalf.y(), upperCircle[i].z() - dirHalf.z());
}
for (int i = 3; i >= 0; --i)
{
glVertex3f(lowerCircle[i].x() + dirHalf.x(), lowerCircle[i].y() + dirHalf.y(), lowerCircle[i].z() + dirHalf.z());
glVertex3f(lowerCircle[i].x() - dirHalf.x(), lowerCircle[i].y() - dirHalf.y(), lowerCircle[i].z() - dirHalf.z());
}
glVertex3f(upperCircle[0].x() + dirHalf.x(), upperCircle[0].y() + dirHalf.y(), upperCircle[0].z() + dirHalf.z());
glVertex3f(upperCircle[0].x() - dirHalf.x(), upperCircle[0].y() - dirHalf.y(), upperCircle[0].z() - dirHalf.z());
glEnd();
glBegin(GL_QUAD_STRIP);
for (int i = 0; i < 4; ++i)
{
glVertex3f(lowerCircle[i].x() + dirHalf.x(), lowerCircle[i].y() + dirHalf.y(), lowerCircle[i].z() + dirHalf.z());
glVertex3f(upperCircle[i].x() + dirHalf.x(), upperCircle[i].y() + dirHalf.y(), upperCircle[i].z() + dirHalf.z());
}
for (int i = 3; i >= 0; --i)
{
glVertex3f(lowerCircle[i].x() - dirHalf.x(), lowerCircle[i].y() - dirHalf.y(), lowerCircle[i].z() - dirHalf.z());
glVertex3f(upperCircle[i].x() - dirHalf.x(), upperCircle[i].y() - dirHalf.y(), upperCircle[i].z() - dirHalf.z());
}
glEnd();
}
/* glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4f(0.0, 0.0, 0.0, 1.0);
// arrow pointing upwards
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointUp.x() + up.x()*2.0, basePointUp.y() + up.y()*2.0, basePointUp.z() + up.z()*2.0);
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glVertex3f(basePointUp.x() + right.x(), basePointUp.y() + right.y(), basePointUp.z() + right.z());
glVertex3f(basePointUp.x() - dir.x(), basePointUp.y() - dir.y(), basePointUp.z() - dir.z());
glVertex3f(basePointUp.x() - right.x(), basePointUp.y() - right.y(), basePointUp.z() - right.z());
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glEnd();
// arrow pointing to the right
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointRight.x() + right.x()*2.0, basePointRight.y() + right.y()*2.0, basePointRight.z() + right.z()*2.0);
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glVertex3f(basePointRight.x() - up.x(), basePointRight.y() - up.y(), basePointRight.z() - up.z());
glVertex3f(basePointRight.x() - dir.x(), basePointRight.y() - dir.y(), basePointRight.z() - dir.z());
glVertex3f(basePointRight.x() + up.x(), basePointRight.y() + up.y(), basePointRight.z() + up.z());
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glEnd();*/
}
void GLCanvas::renderSingleSensor() const
{
// Vector3 lookAt = Vector3(0.0f, 0.0f, 0.0f);
// Vector3 lookAt = _cameraPosition + _cameraFront;
// gluLookAt(_cameraPosition.x(), _cameraPosition.y(), _cameraPosition.z(),
// lookAt.x() , lookAt.y() , lookAt.z(),
// _cameraUp.x() , _cameraUp.y() , _cameraUp.z());
gluLookAt(0.0f, 0.0f, _cameraPosition.z(),
0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
Vector3 dir = _sensorOrientation.rotate(Vector3(1.0f, 0.0f, 0.0f));
Vector3 up = 0.25 * _sensorOrientation.rotate(Vector3(0.0f, 1.0f, 0.0f));
Vector3 right = 0.5 * _sensorOrientation.rotate(Vector3(0.0f, 0.0f, 1.0f));
Vector3 frontUpRight = dir + up + right;
Vector3 frontDownRight = dir - up + right;
Vector3 frontUpLeft = dir + up - right;
Vector3 frontDownLeft = dir - up - right;
Vector3 backUpRight = -dir + up + right;
Vector3 backDownRight = -dir - up + right;
Vector3 backUpLeft = -dir + up - right;
Vector3 backDownLeft = -dir - up - right;
glBegin(GL_QUADS);
glColor3f(0.8, 0.5, 0.5);
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glColor3f(0.5, 0.7, 0.5);
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glColor3f(0.5, 0.5, 0.7);
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glColor3f(0.7, 0.7, 0.7);
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glEnd();
// scale slightly to ensure the lines are visible
glScalef(1.001f, 1.001f, 1.001f);
glLineWidth(1.0f);
glColor3f(0.0, 0.0, 0.0);
glBegin(GL_LINE_STRIP);
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glEnd();
glBegin(GL_LINES);
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glEnd();
glLineWidth(2.0f);
// draw global coordinate system
glBegin(GL_LINES);
glColor3f(1.0, 0.0, 0.0);
glVertex3f(-1.5, -1.0, -1.0);
glVertex3f(1.0, -1.0, -1.0);
glColor3f(0.0, 1.0, 0.0);
glVertex3f(-1.5, -1.0, -1.0);
glVertex3f(-1.5, 1.0, -1.0);
glColor3f(0.0, 0.0, 1.0);
glVertex3f(-1.5, -1.0, -1.0);
glVertex3f(-1.5, -1.0, 1.0);
glEnd();
// draw global coordinate system
// glBegin(GL_LINES);
// glColor3f(1.0, 0.0, 0.0);
// glVertex3f(0.0, 0.0, 0.0);
// glVertex3f(1.5, 00.0, 0.0);
//
// glColor3f(0.0, 1.0, 0.0);
// glVertex3f(0.0, 0.0, 0.0);
// glVertex3f(0.0, 1.5, 0.0);
//
// glColor3f(0.0, 0.0, 1.0);
// glVertex3f(0.0, 0.0, 0.0);
// glVertex3f(0.0, 0.0, 1.5);
// glEnd();
glLineWidth(1.0f);
}
void GLCanvas::renderSingleJoint() const
{
Vector3 lookAt = _cameraPosition + _cameraFront;
gluLookAt(_cameraPosition.x(), _cameraPosition.y(), _cameraPosition.z(),
lookAt.x() , lookAt.y() , lookAt.z(),
_cameraUp.x() , _cameraUp.y() , _cameraUp.z());
glBegin(GL_LINES);
glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(1.0f, 0.0f, 0.0f);
glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 1.0f, 0.0f);
glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, 1.0f);
glEnd();
if (_constraint != nullptr)
{
drawJoint(*_constraint, 1.0f);
}
}
// Initialization of all OpenGL specific parameters.
void GLCanvas::InitGL()
{
SetCurrent(*_GLRC);
glClearColor(0.0, 0.0, 0.0, 0.0);
//glClearColor(1.0, 1.0, 1.0, 1.0);
glEnable(GL_DEPTH_TEST);
glEnable(GL_POINT_SMOOTH);
glEnable(GL_BLEND);
glEnable(GL_CULL_FACE);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glShadeModel(GL_SMOOTH);
}
