///////////////////////////////////////////////////////////////////////////////
// Draw the scene
//
void DrawWorld(GLfloat yRot, GLfloat xPos)
{
M3DMatrix44f mCamera;
modelViewMatrix.GetMatrix(mCamera);
// Need light position relative to the Camera
M3DVector4f vLightTransformed;
m3dTransformVector4(vLightTransformed, vLightPos, mCamera);
// Draw stuff relative to the camera
modelViewMatrix.PushMatrix();
modelViewMatrix.Translate(0.0f, 0.2f, -2.5f);
modelViewMatrix.Translate(xPos, 0.0f, 0.0f);
modelViewMatrix.Rotate(yRot, 0.0f, 1.0f, 0.0f);
shaderManager.UseStockShader(GLT_SHADER_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeline.GetProjectionMatrix(),
vLightTransformed, vGreen, 0);
torusBatch.Draw();
modelViewMatrix.PopMatrix();
}
void animate() {
frameNo++;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
glUseProgram(shader);
//cameraAngleX += randf() * 5.0;
//cameraAngleY += randf() * 2.7;
//cameraAngleZ += randf() * 50.0;
cameraBothZ += randf() * 10.0;
matrixStack.LoadIdentity();
matrixStack.PushMatrix();
matrixStack.Scale(0.1,0.1,0.1);
//Perspektywa + krêcenie kamery
matrixStack.Rotate(40, 1, 0, 0);
matrixStack.Rotate(cameraBothZ, 0, 0, 1);
glUniformMatrix4fv(PMatrixLocation,1,GL_FALSE,matrixStack.GetMatrix());
float ambient[] = {0.5,0.2,0.2}, diffuse[] = {0.5,0.2,1}, specular[] = {0.2,1,0.2};
material.setMaterial(ambient, diffuse, specular, 1);
float color[] = {1,1,1}, position[] = {0,0,-1};
light.setLight(position, color, 180, 1, 1, 2);
float ambientLight[] = {1,1,1};
glUniform3fv(ambientLightLocation, 1, ambientLight);
float small_color[] = {1,1,1}, small_position[] = {5 + 2*cos(frameNo/180.0*PI/2.0), 5 + 2*sin(frameNo/180.0*PI/2.0), -1};
small_light.setLight(small_position, small_color, 180, 1, 1, 2);
matrixStack.PopMatrix();
//siatka
pushSiatka();
float ambient2[] = {0.2,0.2,0.2};
material.setMaterial(ambient2, diffuse, specular, 1);
RenderDwudziestoscian(5,5,-2,0.8);
RenderDwudziestoscian(-5,-5,-2,0.5);
RenderDwudziestoscian(small_position[0], small_position[1], small_position[2], 0.2);
glutSwapBuffers();
}
void pushSiatka() {
glUniformMatrix4fv(MVMatrixLocation,1,GL_FALSE,matrixStack.GetMatrix());
glBegin(GL_TRIANGLES);
glVertexAttrib4f(GLT_ATTRIBUTE_COLOR, 0.2, 0.2, 0.2, 1.0);
for(int i = -10; i<=10; i++) {
for(int j = -10; j<=10; j++) {
float v1[] = {i,j,0};
float v2[] = {i+1,j,0};
float v3[] = {i,j+1,0};
float v4[] = {i+1,j+1,0};
TriangleFace(v1,v2,v3);
TriangleFace(v2,v4,v3);
}
}
glEnd();
}
///////////////////////////////////////////////////////////////////////////////
// Draw the scene
//
void DrawWorld(GLfloat yRot)
{
M3DMatrix44f mCamera;
modelViewMatrix.GetMatrix(mCamera);
// Need light position relative to the Camera
M3DVector4f vLightTransformed;
m3dTransformVector4(vLightTransformed, vLightPos, mCamera);
// Draw the light source as a small white unshaded sphere
modelViewMatrix.PushMatrix();
modelViewMatrix.Translatev(vLightPos);
if(bUseFBO)
UseProcessProgram(vLightPos, vWhite, -1);
else
shaderManager.UseStockShader(GLT_SHADER_FLAT, transformPipeline.GetModelViewProjectionMatrix(), vWhite);
sphereBatch.Draw();
modelViewMatrix.PopMatrix();
// Draw stuff relative to the camera
modelViewMatrix.PushMatrix();
modelViewMatrix.Translate(0.0f, 0.2f, -2.5f);
modelViewMatrix.PushMatrix();
modelViewMatrix.Rotate(yRot, 0.0f, 1.0f, 0.0f);
modelViewMatrix.Translate(0.0, (GLfloat)-0.60, 0.0);
modelViewMatrix.Scale((GLfloat)0.02, (GLfloat)0.006, (GLfloat)0.02);
glBindTexture(GL_TEXTURE_2D, ninjaTex[0]);
if(bUseFBO)
{
UseProcessProgram(vLightTransformed, vWhite, 0);
}
else
{
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_REPLACE, transformPipeline.GetModelViewProjectionMatrix(), 0);
}
ninja.Render(0,0);
modelViewMatrix.PopMatrix();
modelViewMatrix.PopMatrix();
}
void DrawSongAndDance(GLfloat yRot) // Called to draw dancing objects
{
static GLfloat vWhite[] = { 1.0f, 1.0f, 1.0f, 1.0f };
static GLfloat vLightPos[] = { 0.0f, 3.0f, 0.0f, 1.0f };
// Get the light position in eye space
M3DVector4f vLightTransformed;
M3DMatrix44f mCamera;
modelViewMatrix.GetMatrix(mCamera);
m3dTransformVector4(vLightTransformed, vLightPos, mCamera);
// Draw the light source
modelViewMatrix.PushMatrix();
modelViewMatrix.Translatev(vLightPos);
shaderManager.UseStockShader(GLT_SHADER_FLAT,
transformPipeLine.GetModelViewProjectionMatrix(),
vWhite);
sphereBatch.Draw();
modelViewMatrix.PopMatrix();
glBindTexture(GL_TEXTURE_2D, uiTextures[2]);
for (int i = 0; i < NUM_SPHERES; i++) {
modelViewMatrix.PushMatrix();
modelViewMatrix.MultMatrix(spheres[i]);
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeLine.GetProjectionMatrix(),
vLightTransformed,
vWhite,
0);
sphereBatch.Draw();
modelViewMatrix.PopMatrix();
}
// Song and dance
modelViewMatrix.Translate(0.0f, 0.2f, -2.5f);
modelViewMatrix.PushMatrix(); // Saves the translated origin
modelViewMatrix.Rotate(yRot, 0.0f, 1.0f, 0.0f);
// Draw stuff relative to the camera
glBindTexture(GL_TEXTURE_2D, uiTextures[1]);
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeLine.GetProjectionMatrix(),
vLightTransformed,
vWhite,
0);
torusBatch.Draw();
modelViewMatrix.PopMatrix(); // Erased the rotate
modelViewMatrix.Rotate(yRot * -2.0f, 0.0f, 1.0f, 0.0f);
modelViewMatrix.Translate(0.8f, 0.0f, 0.0f);
glBindTexture(GL_TEXTURE_2D, uiTextures[2]);
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeLine.GetProjectionMatrix(),
vLightTransformed,
vWhite,
0);
sphereBatch.Draw();
}
///////////////////////////////////////////////////////////////////////////////
// Render the block
void RenderBlock(void)
{
GLfloat vRed[] = { 1.0f, 0.0f, 0.0f, 1.0f};
GLfloat vWhite[] = { 1.0f, 1.0f, 1.0f, 1.0f };
switch(nStep)
{
// Wire frame
case 0:
glEnable(GL_BLEND);
glEnable(GL_LINE_SMOOTH);
shaderManager.UseStockShader(GLT_SHADER_FLAT, transformPipeline.GetModelViewProjectionMatrix(), vRed);
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glDisable(GL_CULL_FACE);
// Draw the cube
cubeBatch.Draw();
break;
// Wire frame, but not the back side... we also want the block to be in the stencil buffer
case 1:
shaderManager.UseStockShader(GLT_SHADER_FLAT, transformPipeline.GetModelViewProjectionMatrix(), vRed);
// Draw (back side) solid block in stencil buffer
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glEnable(GL_STENCIL_TEST);
glStencilFunc(GL_NEVER, 0, 0);
glStencilOp(GL_INCR, GL_INCR, GL_INCR);
glFrontFace(GL_CW);
cubeBatch.Draw();
glFrontFace(GL_CCW);
glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
glDisable(GL_STENCIL_TEST);
glEnable(GL_BLEND);
glEnable(GL_LINE_SMOOTH);
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
// Draw the front side cube
cubeBatch.Draw();
break;
// Solid
case 2:
shaderManager.UseStockShader(GLT_SHADER_FLAT, transformPipeline.GetModelViewProjectionMatrix(), vRed);
// Draw the cube
cubeBatch.Draw();
break;
// Lit
case 3:
shaderManager.UseStockShader(GLT_SHADER_POINT_LIGHT_DIFF, modelViewMatrix.GetMatrix(),
projectionMatrix.GetMatrix(), vLightPos, vRed);
// Draw the cube
cubeBatch.Draw();
break;
// Textured & Lit
case 4:
case 5:
default:
glBindTexture(GL_TEXTURE_2D, textures[2]);
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF, modelViewMatrix.GetMatrix(),
projectionMatrix.GetMatrix(), vLightPos, vWhite, 0);
glBindTexture(GL_TEXTURE_2D, textures[1]);
topBlock.Draw();
glBindTexture(GL_TEXTURE_2D, textures[2]);
frontBlock.Draw();
glBindTexture(GL_TEXTURE_2D, textures[3]);
leftBlock.Draw();
break;
}
// Put everything back
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glEnable(GL_CULL_FACE);
glDisable(GL_BLEND);
glDisable(GL_LINE_SMOOTH);
glDisable(GL_STENCIL_TEST);
}
///////////////////////////////////////////////////////////////////////////////
// Render a frame. The owning framework is responsible for buffer swaps,
// flushes, etc.
void RenderScene(void)
{
static CStopWatch animationTimer;
float yRot = animationTimer.GetElapsedSeconds() * 60.0f;
M3DVector3f vCameraPos;
M3DVector3f vCameraForward;
M3DVector3f vMirrorPos;
M3DVector3f vMirrorForward;
cameraFrame.GetOrigin(vCameraPos);
cameraFrame.GetForwardVector(vCameraForward);
// Set position of mirror frame (camera)
vMirrorPos[0] = 0.0;
vMirrorPos[1] = 0.1f;
vMirrorPos[2] = -6.0f; // view pos is actually behind mirror
mirrorFrame.SetOrigin(vMirrorPos);
// Calculate direction of mirror frame (camera)
// Because the position of the mirror is known relative to the origin
// find the direction vector by adding the mirror offset to the vector
// of the viewer-origin
vMirrorForward[0] = vCameraPos[0];
vMirrorForward[1] = vCameraPos[1];
vMirrorForward[2] = (vCameraPos[2] + 5);
m3dNormalizeVector3(vMirrorForward);
mirrorFrame.SetForwardVector(vMirrorForward);
// first render from the mirrors perspective
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, fboName);
glDrawBuffers(1, fboBuffs);
glViewport(0, 0, mirrorTexWidth, mirrorTexHeight);
// Draw scene from the perspective of the mirror camera
modelViewMatrix.PushMatrix();
M3DMatrix44f mMirrorView;
mirrorFrame.GetCameraMatrix(mMirrorView);
modelViewMatrix.MultMatrix(mMirrorView);
// Flip the mirror camera horizontally for the reflection
modelViewMatrix.Scale(-1.0f, 1.0f, 1.0f);
glBindTexture(GL_TEXTURE_2D, textures[0]); // Marble
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_MODULATE, transformPipeline.GetModelViewProjectionMatrix(), vWhite, 0);
floorBatch.Draw();
DrawWorld(yRot);
// Now draw a cylinder representing the viewer
M3DVector4f vLightTransformed;
modelViewMatrix.GetMatrix(mMirrorView);
m3dTransformVector4(vLightTransformed, vLightPos, mMirrorView);
modelViewMatrix.Translate(vCameraPos[0],vCameraPos[1]-0.8f,vCameraPos[2]-1.0f);
modelViewMatrix.Rotate(-90.0f, 1.0f, 0.0f, 0.0f);
shaderManager.UseStockShader(GLT_SHADER_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeline.GetProjectionMatrix(),
vLightTransformed, vBlue, 0);
cylinderBatch.Draw();
modelViewMatrix.PopMatrix();
// Reset FBO. Draw world again from the real cameras perspective
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
glDrawBuffers(1, windowBuff);
glViewport(0, 0, screenWidth, screenHeight);
modelViewMatrix.PushMatrix();
M3DMatrix44f mCamera;
cameraFrame.GetCameraMatrix(mCamera);
modelViewMatrix.MultMatrix(mCamera);
glBindTexture(GL_TEXTURE_2D, textures[0]); // Marble
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_MODULATE, transformPipeline.GetModelViewProjectionMatrix(), vWhite, 0);
floorBatch.Draw();
DrawWorld(yRot);
// Now draw the mirror surfaces
modelViewMatrix.PushMatrix();
modelViewMatrix.Translate(0.0f, -0.4f, -5.0f);
if(vCameraPos[2] > -5.0)
{
glBindTexture(GL_TEXTURE_2D, mirrorTexture); // Reflection
shaderManager.UseStockShader(GLT_SHADER_TEXTURE_REPLACE, transformPipeline.GetModelViewProjectionMatrix(), 0);
}
else
{
// If the camera is behind the mirror, just draw black
shaderManager.UseStockShader(GLT_SHADER_FLAT, transformPipeline.GetModelViewProjectionMatrix(), vBlack);
}
mirrorBatch.Draw();
shaderManager.UseStockShader(GLT_SHADER_FLAT, transformPipeline.GetModelViewProjectionMatrix(), vGrey);
mirrorBorderBatch.Draw();
modelViewMatrix.PopMatrix();
modelViewMatrix.PopMatrix();
// Do the buffer Swap
glutSwapBuffers();
// Do it again
glutPostRedisplay();
}
// called to draw dancing objects
static void DrawSongAndDance(GLfloat yRot)
{
static GLfloat vWhite[] = { 1.0f, 1.0f, 1.0f, 1.0f };
static GLfloat vLightPos[] = { 0.0f, 3.0f, 0.0f, 1.0f };
// get the light position in eye space
M3DMatrix44f mCamera;
modelViewMatrix.GetMatrix(mCamera);
M3DVector4f vLightTransformed;
m3dTransformVector4(vLightTransformed, vLightPos, mCamera);
// draw the light source
modelViewMatrix.push();
modelViewMatrix.moveTo(vLightPos);
shaderManager.useStockShader(GLT_SHADER_FLAT, transformPipeline.GetMVPMatrix(),
vWhite);
sphereBatch.draw();
modelViewMatrix.pop();
glBindTexture(GL_TEXTURE_2D, uiTextures[2]);
for (int i = 0; i < NUM_SPHERES; ++i) {
modelViewMatrix.push();
modelViewMatrix.MultMatrix(spheres[i]);
shaderManager.useStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeline.GetProjectionMatrix(),
vLightTransformed,
vWhite,
0);
sphereBatch.draw();
modelViewMatrix.pop();
}
// song and dance
modelViewMatrix.moveTo(0.0f, 0.2f, -2.5f);
modelViewMatrix.push(); // save the translated origin
modelViewMatrix.rotateTo(yRot, 0.0f, 1.0f, 0.0f);
// draw stuff relative to the camera
glBindTexture(GL_TEXTURE_2D, uiTextures[1]);
shaderManager.useStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeline.GetProjectionMatrix(),
vLightTransformed,
vWhite,
0);
torusBatch.draw();
modelViewMatrix.pop(); // erased the rotate
modelViewMatrix.rotateTo(yRot * -2.0f, 0.0f, 1.0f, 0.0f);
modelViewMatrix.moveTo(0.8f, 0.0f, 0.0f);
glBindTexture(GL_TEXTURE_2D, uiTextures[2]);
shaderManager.useStockShader(GLT_SHADER_TEXTURE_POINT_LIGHT_DIFF,
modelViewMatrix.GetMatrix(),
transformPipeline.GetProjectionMatrix(),
vLightTransformed,
vWhite,
0);
sphereBatch.draw();
}
void Gripper::render(GLMatrixStack &modelViewStack, GLMatrixStack &projectionStack, float *phantomMat, GLShaderManager &shaderManager){
modelViewStack.PushMatrix();
//modelViewStack.Translate(position[0] + size[0]*0.5f, position[1] + size[1]*0.5f, position[2] + size[2]*0.5f);
modelViewStack.MultMatrix(phantomMat);
modelViewStack.Scale(scalar, scalar, scalar);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), curColor);
sphereBatch.Draw();
modelViewStack.PushMatrix();
modelViewStack.PushMatrix();
modelViewStack.Rotate(90, 0, 1, 0);
modelViewStack.Scale(0.5, 0.5, endEffector[RED].curPos);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), curColor);
cylBatch.Draw();
modelViewStack.PopMatrix();
modelViewStack.Translate(endEffector[RED].curPos, 0, 0.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vRed);
sphereBatch.Draw();
modelViewStack.Translate(0.0, 0, -8.0);
modelViewStack.PushMatrix();
modelViewStack.Scale(0.5, 0.5, 8.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), curColor);
cylBatch.Draw();
modelViewStack.PopMatrix();
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vRed);
sphereBatch.Draw();
modelViewStack.PushMatrix();
modelViewStack.Translate(0.0, 2.0, -2.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vRed);
sphereBatch.Draw();
modelViewStack.Translate(0.0, -4.0, 0.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vRed);
sphereBatch.Draw();
modelViewStack.Translate(0.0, 2.0, -2.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vRed);
sphereBatch.Draw();
modelViewStack.PopMatrix();
modelViewStack.PopMatrix();
modelViewStack.PushMatrix();
modelViewStack.PushMatrix();
modelViewStack.Rotate(-90, 0, 1, 0);
modelViewStack.Scale(0.5, 0.5, endEffector[GREEN].curPos);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), curColor);
cylBatch.Draw();
modelViewStack.PopMatrix();
modelViewStack.Translate(-endEffector[GREEN].curPos, 0, 0.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vGreen);
sphereBatch.Draw();
modelViewStack.Translate(0.0, 0, -8.0);
modelViewStack.PushMatrix();
modelViewStack.Scale(0.5, 0.5, 8.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), curColor);
cylBatch.Draw();
modelViewStack.PopMatrix();
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vGreen);
sphereBatch.Draw();
modelViewStack.PushMatrix();
modelViewStack.Translate(0.0, 2.0, -2.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vGreen);
sphereBatch.Draw();
modelViewStack.Translate(0.0, -4.0, 0.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vGreen);
sphereBatch.Draw();
modelViewStack.Translate(0.0, 2.0, -2.0);
shaderManager.UseStockShader(GLT_SHADER_DEFAULT_LIGHT, modelViewStack.GetMatrix(), projectionStack.GetMatrix(), vGreen);
sphereBatch.Draw();
modelViewStack.PopMatrix();
modelViewStack.PopMatrix();
modelViewStack.PopMatrix();
}
void Gripper::environmentCalc(GLMatrixStack &modelViewStack, float phantomMat[16]){
phidgets.sampleAll();
for(int i = 0; i < 2; ++i){
if(phidgets.getRaw(i) > 500)
return;
if(phidgets.getRaw(i) < 0)
return;
}
endEffector[RED].currentRaw = phidgets.getRaw(0);
endEffector[GREEN].currentRaw = phidgets.getRaw(1);
for(int i = 0; i < 2; ++i){
END_EFFECTOR *ee = &(endEffector[i]);
ee->maxRaw = max(ee->maxRaw, ee->currentRaw);
ee->minRaw = min(ee->minRaw, ee->currentRaw);
double range = ee->maxRaw - ee->minRaw;
double raw = ee->currentRaw - ee->minRaw;
ee->gripperForce = max((1.0 - (raw/range)), 0);
ee->curPos = ee->curPos - (ee->gripperForce - ee->restoreForce - ee->contactForce) * ee->scalar;
ee->curPos = min(ee->maxRange, ee->curPos);
ee->curPos = max(ee->minRange, ee->curPos);
//Dprint::add("[%d] range = %.2f, raw = %.2f, maxRaw = %d, minRaw = %d, curPos = %.2f", i, range, raw, ee->maxRaw, ee->minRaw, ee->curPos);
}
modelViewStack.PushMatrix();
modelViewStack.LoadIdentity();
modelViewStack.MultMatrix(phantomMat);
modelViewStack.Scale(scalar, scalar, scalar);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[YELLOW]);
modelViewStack.PushMatrix();
modelViewStack.Translate(endEffector[RED].curPos, 0, 0.0);
modelViewStack.Translate(0.0, 0, -8.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[RED]);
modelViewStack.PushMatrix();
modelViewStack.Translate(0.0, 2.0, -2.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[RED2]);
modelViewStack.Translate(0.0, -4.0, 0.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[RED3]);
modelViewStack.Translate(0.0, 2.0, -2.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[RED4]);
modelViewStack.PopMatrix();
modelViewStack.PopMatrix();
modelViewStack.PushMatrix();
modelViewStack.Translate(-endEffector[GREEN].curPos, 0, 0.0);
modelViewStack.Translate(0.0, 0, -8.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[GREEN]);
modelViewStack.PushMatrix();
modelViewStack.Translate(0.0, 2.0, -2.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[GREEN2]);
modelViewStack.Translate(0.0, -4.0, 0.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[GREEN3]);
modelViewStack.Translate(0.0, 2.0, -2.0);
modelViewStack.GetMatrix(matrix);
setPosFromMatrix(positions[GREEN4]);
modelViewStack.PopMatrix();
modelViewStack.PopMatrix();
modelViewStack.PopMatrix();
for(int i = 0; i < NUM_END_EFFECTORS; ++i){
offsets[i][0] = positions[i][0] - positions[YELLOW][0];
offsets[i][1] = positions[i][1] - positions[YELLOW][1];
offsets[i][2] = positions[i][2] - positions[YELLOW][2];
}
}