C++ GLMatrixStack::GetMatrix方法代码示例

///////////////////////////////////////////////////////////////////////////////
// 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();
    
//.........这里部分代码省略.........

// 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];
	}
}