OpenGL de プログラミング

#include <sstream>
#include <string>
#include <iostream>
#include <cv.h>
#include <GL/glut.h>

//------------ 各種外部変数 --------------------//
double PositionXYZ[]={1,2,1};

//-------------- プロトタイプ宣言 -------------------//
void display();
void reshape(int w, int h);
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state,int x, int y);
void DrawXYZ();
void GetWinCoordinate();
void ChangeTitleBar(const int x, const int y);
int MygluProject(double objx, double objy, double objz, cv::Mat &PM, const int viewport[4],double *winx,double *winy,double *winz);

//-------------- OpenGLの初期設定 -----------------//
void GLUT_INIT()
{
	glutInitDisplayMode(GLUT_RGBA| GLUT_DOUBLE | GLUT_DEPTH);
	glutInitWindowSize(640,480);
	glutCreateWindow("Object to Window Coord");
}

void GLUT_CALL_FUNC()
{
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
}

void MY_INIT()
{
	glutSetCursor(GLUT_CURSOR_CROSSHAIR);
	glClearColor(1.0, 1.0, 1.0, 1.0);
	std::cout << "Press [w] for Obj to Win Coordinate\n";

	glEnable(GL_DEPTH_TEST);
	glPointSize(2);
}


//-------------- メイン関数 ----------------------//
int main(int argc, char **argv)
{
	glutInit(&argc,argv);
	GLUT_INIT();

	GLUT_CALL_FUNC();
	MY_INIT();
	glutMainLoop();

	return 0;
}

//------------ ここから各種コールバック --------------------//
void display()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glLoadIdentity();
	gluLookAt(3.0, 4.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

	DrawXYZ();

	glColor3f(1,0,1);
	glBegin(GL_POINTS);
	glVertex3dv(PositionXYZ);
	glEnd();
	glColor3f(1,1,1);

	glutSwapBuffers();
}

void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(30.0, (double)w / (double)h, 1.0, 100.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(3.0, 4.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
}

void keyboard(unsigned char key, int x, int y)
{
	if(key == 'w'){
		GetWinCoordinate();
		std::cout << "\n<<< press [w] >>>\n";
	}
}

void mouse(int button, int state,int x, int y)
{
	if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN){
		ChangeTitleBar(x, y);
	}
}


//------------- ここから各種関数 -----------------//
void DrawXYZ()
{
	glBegin(GL_LINES);

	glColor3d(0,1,0);//x
	glVertex2d(-100,0);
	glVertex2d(100, 0);

	glColor3d(1,0,0);//y
	glVertex2d(0,0);
	glVertex2d(0,100);

	glColor3d(0,0,1);//z
	glVertex3d(0,0,-100);
	glVertex3d(0,0, 100);
	glEnd();
}




void GetWinCoordinate()
{
	static double modelview[16];
	glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
	static double projection[16];
	glGetDoublev(GL_PROJECTION_MATRIX, projection);
	static int viewport[4];
	glGetIntegerv(GL_VIEWPORT, viewport);


	cv::Mat Model(4,4,CV_64FC1,modelview);
	cv::Mat Proj(4,4,CV_64FC1,projection);

	cv::Mat PM = Model *Proj;

	static double winX, winY, winZ;
	//gluProject(PositionXYZ[0],PositionXYZ[1],PositionXYZ[2],modelview,projection,viewport,&winX,&winY,&winZ);
	MygluProject(PositionXYZ[0],PositionXYZ[1],PositionXYZ[2],PM,viewport,&winX,&winY,&winZ);

	std::cout <<"ウィンドウ座標 : "<< winX << "," << viewport[3] - winY << "\n";
}


void ChangeTitleBar(const int x, const int y)
{
	std::stringstream title;
	title << "(" << x << "," << y << ")";

	glutSetWindowTitle( std::string( title.str() ).c_str());
}


int MygluProject(double objx, double objy, double objz, cv::Mat &PM, const int viewport[4],double *winx,double *winy,double *winz)
{
	double in[4]= {objx,objy,objz,1.0};
	cv::Mat v(4,1,CV_64FC1,in);

	cv::transpose(PM,PM);//転置
	cv::Mat v_dash = PM * v;

	if(v_dash.at<double>(3,0) == 0.0)
	{
		return GL_FALSE;
	}

	v_dash = v_dash / v_dash.at<double>(3,0);

	*winx = viewport[0]+(1+v_dash.at<double>(0,0))*viewport[2]/2;
	*winy = viewport[1]+(1+v_dash.at<double>(1,0))*viewport[3]/2;
	*winz = (1+v_dash.at<double>(2,0))/2;

	return GL_TRUE;
}