Target Following Control with OpenGL

#include "stdafx.h"
#include "gl\glut.h"
#include <math.h>
#include <gl\gl.h>
#include <math.h>
//To control the camera use keys:q,w,e,a,s,d,z,x

//angle of rotation
float xpos = 0, ypos = 0, zpos = 60, xrot = 0, yrot = 0, angle=0.0;

//positions of the cube
float positionz[10];
float positionx[10];
float Position[3] = { -1, 4, 2};


void cubepositions (void) { //set the positions of the cube
	for (int i=0;i<3;i++)
	{
		positionz[i] = i;
		positionx[i] = i;
	}
}
int getOutcode(double x,double y)
{
	int out = 0;
 	if (x < 0) {
         out=1;
     } else if (x > 500) {
         out=2;
     }

     if (y < 0) {
         out =3;
     } else if (y > 500) {
         out =4;
     }
	return out;
}
GLfloat* getIntersectingPoint(double theta)
{
	GLfloat* IPoint=new GLfloat(0,0);

	double cx = 250;//Centru chenariului x si y
    double cy = 250;
    double W = 500/2;//Latimea ecranului
    double H = 500/2;//Inaltimea ecranului
    double R = sqrt((W)*(W)+(H)*(H));
    double x = cx + R *cosf(theta);
    double y = cy + R *sinf(theta);
    int outcode = getOutcode(x, y);
    switch(outcode)
    {
        case 3:
            IPoint[0]= cx - H * ((x - cx)/(y - cy));
            IPoint[1]= cy - H;
            break;
        case 1:
            IPoint[0]= cx - W;
            IPoint[1] = cy - W * ((y - cy)/(x - cx));
            break;
        case 4:
            IPoint[0]= cx + H * ((x - cx)/(y - cy));
            IPoint[1]= cy + H;
            break;
        case 2:
            IPoint[0] = cx + W;
            IPoint[1]= cy + W * ((y - cy)/(x - cx));
            break;
    }
	return IPoint;
}

void getDirection()
{
	GLfloat Matrix[16];
	GLfloat TargetPosition[3];
	GLfloat TargetSum;
	GLfloat NTargetPosition[2];

	glGetFloatv(GL_MODELVIEW_MATRIX, Matrix);
	TargetPosition[0] = Position[0] * Matrix[0] + Position[1] * Matrix[4] + Position[2] * Matrix[8] + 1.0f * Matrix[12];
	TargetPosition[1] = Position[0]* Matrix[1] + Position[1] * Matrix[5] + Position[2] * Matrix[9] + 1.0f * Matrix[13];
	TargetPosition[2] = Position[0]* Matrix[2] + Position[1]* Matrix[6] + Position[2] * Matrix[10] + 1.0f * Matrix[14];
	TargetSum=sqrt(TargetPosition[0] *TargetPosition[0]+TargetPosition[1]*TargetPosition[1]);
	NTargetPosition[0]=(TargetPosition[0]/TargetSum);//raza offset din centru ecran inclusa acum
	NTargetPosition[1]=(TargetPosition[1]/TargetSum);

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glOrtho(0, 500, 500, 0, -1, 1);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	glTranslatef(250, 250, 0.0);
	
	glColor3f(1.0,1.0,1.0);

	/*
	//Uncomment these lines for an older version of target following
	//Uses a circle to display the target direction
	float radius = 100.0f;
	glBegin(GL_LINE_STRIP);
	float angle;
	for(int i = 0; i < 50; i++) {
		angle = i * 2.0f * 3.141516f / 50.0f;
		glVertex2f(radius * cosf(angle), radius * sinf(angle));
	}
	glVertex2f(radius, 0.0f);
	glEnd();

	glPointSize(5.0f);
	glBegin(GL_POINTS);
	glVertex2f(NTargetPosition[0] * radius, -NTargetPosition[1] * radius);
	glVertex2f(0, 0);
	glEnd();

	glBegin(GL_LINES);
	glVertex2f(NTargetPosition[0] * radius, -NTargetPosition[1] * radius);
	glVertex2f(0, 0);
	glEnd();
    */
    double theta = atan2(NTargetPosition[0], NTargetPosition[1]);
    GLfloat* IPoint=getIntersectingPoint(theta);

    glPointSize(10.0f);
	glBegin(GL_POINTS);
	glVertex2f((IPoint[1] -250)/1.1, (-IPoint[0] +250)/1.1);
	glEnd();

	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();

	}
//draw the cube
void cube (void)
{
	
	glPushMatrix();
	glTranslatef(Position[0], Position[1],Position[2]); //translate the cube
	glutSolidCube(2); //draw the cube
	glPopMatrix();
}

void init (void) {
	cubepositions();
}

void enable (void) {
	glEnable (GL_DEPTH_TEST); //enable the depth testing
	glEnable (GL_LIGHTING); //enable the lighting
	glEnable (GL_LIGHT0); //enable LIGHT0, our Diffuse Light
	glShadeModel (GL_SMOOTH); //set the shader to smooth shader
}

void camera (void) {
	glRotatef(xrot,1.0,0.0,0.0);  //rotate our camera on teh x-axis (left and right)
	glRotatef(yrot,0.0,1.0,0.0);  //rotate our camera on the y-axis (up and down)
	glTranslated(-xpos,-ypos,-zpos); //translate the screen to the position of our camera
}
void reshape (int w, int h) {
	glViewport (0, 0, (GLsizei)w, (GLsizei)h); //set the viewport to the current window specifications
	glMatrixMode (GL_PROJECTION); //set the matrix to projection
	glLoadIdentity ();
	gluPerspective (60, (GLfloat)w / (GLfloat)h, 1.0, 1000.0); //set the perspective (angle of sight, width, height, , depth)
	glMatrixMode (GL_MODELVIEW); //set the matrix back to model
}

void display (void) {
	glClearColor (0.4,0.4,0.4,1.0); //clear the screen to black

	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //clear the color buffer and the depth buffer
	glLoadIdentity();  
	camera();
	enable();

	//glClearColor(0.4f, 0.4f, 0.4f, 1.0f);

	cube(); //call the cube drawing function

	getDirection();

	glutSwapBuffers(); //swap the buffers
	angle++; //increase the angle
}



void keyboard (unsigned char key, int x, int y) {
	if (key=='q')
	{
		xrot += 1;
		if (xrot >360) xrot -= 360;
	}

	if (key=='z')
	{
		xrot -= 1;
		if (xrot < -360) xrot += 360;
	}


	if (key=='w')
	{
		float xrotrad, yrotrad;
		yrotrad = (yrot / 180 * 3.141592654f);
		xrotrad = (xrot / 180 * 3.141592654f); 
		xpos += float(sin(yrotrad)) ;
		zpos -= float(cos(yrotrad)) ;
		ypos -= float(sin(xrotrad)) ;
	}

	if (key=='s')
	{
		float xrotrad, yrotrad;
		yrotrad = (yrot / 180 * 3.141592654f);
		xrotrad = (xrot / 180 * 3.141592654f); 
		xpos -= float(sin(yrotrad));
		zpos += float(cos(yrotrad)) ;
		ypos += float(sin(xrotrad));
	}

	if (key=='d')
	{
		yrot += 1;
		if (yrot >360) yrot -= 360;
	}

	if (key=='a')
	{
		yrot -= 1;
		if (yrot < -360)yrot += 360;
	}
	if (key==27)
	{
		exit(0);
	}
}


int main (int argc, char **argv) {
	glutInit (&argc, argv);
	glutInitDisplayMode (GLUT_DOUBLE | GLUT_DEPTH); //set the display to Double buffer, with depth
	glutInitWindowSize (500, 500); //set the window size
	glutInitWindowPosition (100, 100); //set the position of the window
	glutCreateWindow ("A Follow Target OpenGL Window"); //the caption of the window
	init (); //call the init function
	glutDisplayFunc (display); //use the display function to draw everything
	glutIdleFunc (display); //update any variables in display, display can be changed to anyhing, as long as you move the variables to be updated, in this case, angle++;
	glutReshapeFunc (reshape); //reshape the window accordingly
	glutKeyboardFunc (keyboard); //check the keyboard
	glutMainLoop (); //call the main loop
	return 0;
}