void idle() {
	keyboardEvent();
	int time_ = glutGet(GLUT_ELAPSED_TIME);
	float angle = time_ / 1000.0 * 10;

	Vector3 pos = player.getPosition();
	float angX = player.getRotationX(),
		  angY = player.getRotationY();

	Vector3 cpos = cube.getPosition(),
			cpos2= cube2.getPosition();


	player.updateForce();

	Matrix4 model = Matrix4().Translate(cpos).Rotate(1.0f, cube.getRotation()),
			model2 = Matrix4().Translate(cpos2).Rotate(1.0f, cube2.getRotation());
	Matrix4 projection = Matrix4().Perspective(45.0f, 1.0f*screen_width/screen_height, 0.1f, 50.0f),
			view = Matrix4().lookAt(pos, Vector3(pos.x-cos(angX), pos.y + sin(angY), pos.z + sin(angX)), Vector3(0.0f, 1.0f, 0.0f));
	Matrix4 anim = Matrix4().RotateA(angle*3.0f, Vector3(1, 0, 0)) * 	// X axis
					Matrix4().RotateA(angle*2.0f, Vector3(0, 1, 0)) *	// Y axis
					Matrix4().RotateA(angle*4.0f, Vector3(0, 0, 1));	// Z axis


	Matrix4 m_transform = projection * view * model,
			m_transform2= projection * view * model2;// * anim;

	glUseProgram(cube.getProgram());
	glUniformMatrix4fv(cube.getUniform("m_transform"), 1, GL_FALSE, m_transform);

	glUseProgram(cube2.getProgram());
	glUniformMatrix4fv(cube2.getUniform("m_transform"), 1, GL_FALSE, m_transform2);

	glutPostRedisplay();
}
void onDisplay() {
	if(showPreformance)
		fps++;
	/* Clear the background as white */
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	
	glutSolidCube(1.0);
	glUseProgram(cube.getProgram());
		cube.render();
	glUseProgram(cube2.getProgram());
		cube2.render();
		glTranslatef(0.4f,0.4f,0.4f);
		cube2.render();

	for (int i = 0; i < BALLS; i++) {
		glUseProgram(balls[i].getProgram());
			balls[i].render();
	}
	for (int i = 0; i < DETAIL_GRID*3; i++) {
		glUseProgram(grid[i].getProgram());
			grid[i].render();
	}

	glUseProgram(0);
	PreformanceIRL(1);
	/* Display the result */ 
	glutSwapBuffers();
}
void onDisplay() {
	if(showPreformance)
		fps++;
	/* Clear the background as white */
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	
	glUseProgram(cube.getProgram());
			cube.render();
	glUseProgram(cube2.getProgram());
			cube2.render();

	glUseProgram(0);
	PreformanceIRL(1);
	/* Display the result */ 
	glutSwapBuffers();
}
void idle() {
	keyboardEvent();
	int time_ = glutGet(GLUT_ELAPSED_TIME);
	float angle = time_ / 1000.0 * 10;

	Vector3 pos = player.getPosition();
	float angX = player.getRotationX(),
		  angY = player.getRotationY();

	Vector3 cpos = cube.getPosition(),
			cpos2= cube2.getPosition();

	GLfloat D = cpos.distance(&cpos2),
			W = (cube.dimension() + cube2.dimension())/2;
	float bound = 6.0;
	//if(time_%100 < 10)
	//	gravity = Vector3(rand()%10-5, rand()%10-5, rand()%10-5);
	cvelocity.add(&gravity);
	Vector3 s = cvelocity;
	s.mult(-mass/1000).add(&cpos2);
	bool collides = true;
	if(cpos2.x < -bound && cvelocity.x > 0.0f){
		cvelocity.x = -abs(cvelocity.x)*0.5;
		gravity.x = -abs(gravity.x);
	}
	else if(cpos2.x > bound && cvelocity.x < 0.0f){
		cvelocity.x = abs(cvelocity.x)*0.5;
		gravity.x = abs(gravity.x);
	}
	else if(cpos2.y < -bound && cvelocity.y > 0.0f){
		cvelocity.y = -abs(cvelocity.y)*0.5;
		gravity.y = -abs(gravity.y);
	}
	else if(cpos2.y > bound && cvelocity.y < 0.0f){
		cvelocity.y = abs(cvelocity.y)*0.5;
		gravity.y = abs(gravity.y);
	}
	else if(cpos2.z < -bound && cvelocity.z > 0.0f){
		cvelocity.z = -abs(cvelocity.z)*0.5;
		gravity.z = -abs(gravity.z);
	}
	else if(cpos2.z > bound && cvelocity.z < 0.0f){
		cvelocity.z = abs(cvelocity.z)*0.5;
		gravity.z = abs(gravity.z);
	}
	else{
		collides = false;
	}
	
	if(!collides){
		if(cpos.x + cube.min.x <= cpos2.x + cube2.max.x && cpos.x + cube.max.x >= cpos2.x + cube2.min.x && 
			cpos.z + cube.min.z <= cpos2.z + cube2.max.z && cpos.z + cube.max.z >= cpos2.z + cube2.min.z && 
			cpos.y + cube.min.y <= cpos2.y + cube2.max.y && cpos.y + cube.max.y >= cpos2.y + cube2.min.y){
			collides = true;
			cvelocity.x = cvelocity.y = cvelocity.z = 0.0f;
		}
		
		if(!collides)
			cube2.setPosition(s);
	}

	player.updateForce();
	for (int i = 0; i < BALLS; i++){
		balls[i].updateForce();
		Vector3 ball_pos = balls[i].getPosition(),
				ball_vel = balls[i].dynamics.velocity;
		//printf("x: %f, y: %f, z %f: %f\n", ball_pos.x, ball_pos.y, ball_pos.z, BOUNDARY);
		if(ball_pos.x < -BOUNDARY || ball_pos.x > BOUNDARY)
			balls[i].addVelocity(Vector3(-2 * ball_vel.x, 0.0f, 0.0f));
		if(ball_pos.y < -BOUNDARY || ball_pos.y > BOUNDARY)
			balls[i].addVelocity(Vector3(0.0f, -2 * ball_vel.y, 0.0f));
		if(ball_pos.z < -BOUNDARY || ball_pos.z > BOUNDARY)
			balls[i].addVelocity(Vector3(0.0f, 0.0f, -2 * ball_vel.z));
	}

	Matrix4 model = cube.getTransformMatrix(),
			model2 = cube2.getTransformMatrix();
	Matrix4 projection = Matrix4().Perspective(45.0f, 1.0f*screen_width/screen_height, 0.1f, 50.0f),
			view = Matrix4().lookAt(pos, Vector3(pos.x-cos(angX), pos.y + sin(angY), pos.z + sin(angX)), Vector3(0.0f, 1.0f, 0.0f));
	Matrix4 anim = Matrix4().RotateA(angle*3.0f, Vector3(1, 0, 0)) * 	// X axis
					Matrix4().RotateA(angle*2.0f, Vector3(0, 1, 0)) *	// Y axis
					Matrix4().RotateA(angle*4.0f, Vector3(0, 0, 1));	// Z axis


	Matrix4 m_transform = projection * view;

	glUseProgram(cube.getProgram());
	glUniformMatrix4fv(cube.getUniform("m_transform"), 1, GL_FALSE, m_transform * model);

	glUseProgram(cube2.getProgram());
	glUniformMatrix4fv(cube2.getUniform("m_transform"), 1, GL_FALSE, m_transform * model2);

	for (int i = 0; i < BALLS; i++){
		glUseProgram(balls[i].getProgram());
		glUniformMatrix4fv(
			balls[i].getUniform("m_transform"), 	// m_transform is the transformation of balls[i] in shaders
			1, 										// one uniform-buffer 
			GL_FALSE, 								// no nprmalization
			m_transform * balls[i].getTransformMatrix()
		);
	}
	for (int i = 0; i < DETAIL_GRID*3; i++){
		grid[i].updateForce();
		glUseProgram(grid[i].getProgram());
		glUniformMatrix4fv(
			grid[i].getUniform("m_transform"), 	// m_transform is the transformation of grid[i] in shaders
			1, 										// one uniform-buffer 
			GL_FALSE, 								// no nprmalization
			m_transform * grid[i].getTransformMatrix()
		);
	}

	glutPostRedisplay();
}