#include "../stdafx.h"
#include "CoordinateTrans.h"

CCoordinateTrans::CCoordinateTrans(void)
{
	//初期化
	CAMERA_X = 0.0;		CAMERA_Y = 0.0;		CAMERA_Z = 0.0;
	TILT_SHAFT_X = 0.0;	TILT_SHAFT_Y = 0.0;	TILT_SHAFT_Z = 0.0;
	LEFTARM_X = 0.0;	LEFTARM_Y = 0.0;	LEFTARM_Z = 0.0;
	RIGHTARM_X = 0.0;	RIGHTARM_Y = 0.0;	RIGHTARM_Z = 0.0;

	//パラメータ読み込み
	LoadRobotParameter();
	AllocBuffer();
}

CCoordinateTrans::~CCoordinateTrans(void)
{
	FreeBuffer();
}

void CCoordinateTrans::AllocBuffer()
{
	//メモリ確保
	m_Pcam = cvCreateMat(4, 1, CV_32F);
	m_Probo = cvCreateMat(4, 1, CV_32F);
	m_Pwld = cvCreateMat(4, 1, CV_32F);
	m_Parm = cvCreateMat(4, 1, CV_32F);
	m_Phand = cvCreateMat(4, 1, CV_32F);
	m_Tr_c = cvCreateMat(4, 4, CV_32F);
	m_Tr_1 = cvCreateMat(4, 4, CV_32F);
	m_T1_2 = cvCreateMat(4, 4, CV_32F);
	m_T2_3 = cvCreateMat(4, 4, CV_32F);
	m_T3_c = cvCreateMat(4, 4, CV_32F);
	m_Th_a = cvCreateMat(4, 4, CV_32F);
	m_Th_1 = cvCreateMat(4, 4, CV_32F);
	m_T3_4 = cvCreateMat(4, 4, CV_32F);
	m_T4_5 = cvCreateMat(4, 4, CV_32F);
	m_T5_a = cvCreateMat(4, 4, CV_32F);
}

void CCoordinateTrans::FreeBuffer()
{
	//メモリ開放
	cvReleaseMat( &m_Pcam );
	cvReleaseMat( &m_Probo );
	cvReleaseMat( &m_Pwld );
	cvReleaseMat( &m_Parm );
	cvReleaseMat( &m_Phand );
	cvReleaseMat( &m_Tr_c );
	cvReleaseMat( &m_Tr_1 );
	cvReleaseMat( &m_T1_2 );
	cvReleaseMat( &m_T2_3 );
	cvReleaseMat( &m_T3_c );
	cvReleaseMat( &m_Th_a );
	cvReleaseMat( &m_Th_1 );
	cvReleaseMat( &m_T3_4 );
	cvReleaseMat( &m_T4_5 );
	cvReleaseMat( &m_T5_a );
}

void CCoordinateTrans::LoadRobotParameter(char *filename)
{
	char line[256];
	char buff[256];
	FILE *fp = fopen( filename , "r" );

	if( !fp )
	{
		MessageBox( 0 , "RobotParam.txtの読み込みに失敗" , "座標変換クラス" , 0 );
		return;
	}
	

	while( fgets( line , 256 , fp ) )
	{
		if( strstr( line , "TILT_SHAFT_X" ) )
		{
			sscanf( line , "%s	%lf" , buff , &TILT_SHAFT_X );
		}
		else if( strstr( line , "TILT_SHAFT_Y" ) )
		{
			sscanf( line , "%s	%lf" , buff , &TILT_SHAFT_Y );
		}
		else if( strstr( line , "TILT_SHAFT_Z" ) )
		{
			sscanf( line , "%s	%lf" , buff , &TILT_SHAFT_Z );
		}
		else if( strstr( line , "CAMERA_X" ) )
		{
			sscanf( line , "%s	%lf" , buff , &CAMERA_X );
		}
		else if( strstr( line , "CAMERA_Y" ) )
		{
			sscanf( line , "%s	%lf" , buff , &CAMERA_Y );
		}
		else if( strstr( line , "CAMERA_Z" ) )
		{
			sscanf( line , "%s	%lf" , buff , &CAMERA_Z );
		}
		else if( strstr( line , "LEFTARM_X" ) )
		{
			sscanf( line , "%s	%lf" , buff , &LEFTARM_X );
		}
		else if( strstr( line , "LEFTARM_Y" ) )
		{
			sscanf( line , "%s	%lf" , buff , &LEFTARM_Y );
		}
		else if( strstr( line , "LEFTARM_Z" ) )
		{
			sscanf( line , "%s	%lf" , buff , &LEFTARM_Z );
		}
		else if( strstr( line , "RIGHTARM_X" ) )
		{
			sscanf( line , "%s	%lf" , buff , &RIGHTARM_X );
		}
		else if( strstr( line , "RIGHTARM_Y" ) )
		{
			sscanf( line , "%s	%lf" , buff , &RIGHTARM_Y );
		}
		else if( strstr( line , "RIGHTARM_Z" ) )
		{
			sscanf( line , "%s	%lf" , buff , &RIGHTARM_Z );
		}
	}
		
	fclose(fp);
}

void CCoordinateTrans::Camera2Robot(float pan, float tilt, double camX, double camY, double camZ, double *roboX, double *roboY, double *roboZ, bool flag)
{
	//カメラ座標代入
	cvmSet(m_Pcam, 0, 0, camX);
	cvmSet(m_Pcam, 1, 0, camY);
	cvmSet(m_Pcam, 2, 0, camZ);
	cvmSet(m_Pcam, 3, 0, 1);

	if(flag)
	{
		//同次変換行列作成
		CalcParallelShiftMatrix(m_Tr_1, TILT_SHAFT_X, TILT_SHAFT_Y, TILT_SHAFT_Z);
		CalcTransMatrix(m_T1_2,			0,			  0,		   0,	(pan-90)*PI/180);
		CalcTransMatrix(m_T2_3, -CAMERA_Y,	tilt*PI/180,	CAMERA_Z,				  0);
		CalcTransMatrix(m_T3_c,			0,		  -PI/2,	CAMERA_X,	              0);

		//カメラ⇒ロボットの同次変換行列
		cvmMul( m_Tr_1 , m_T1_2 , m_Tr_c );
		cvmMul( m_Tr_c , m_T2_3 , m_Tr_c );
		cvmMul( m_Tr_c , m_T3_c , m_Tr_c );
	}

	//座標計算
	cvmMul(m_Tr_c, m_Pcam, m_Probo);

	//計算結果代入
	*roboX = cvmGet( m_Probo , 0 , 0 );
	*roboY = cvmGet( m_Probo , 1 , 0 );
	*roboZ = cvmGet( m_Probo , 2 , 0 );
}

void CCoordinateTrans::Robot2World(float roboPosX, float roboPosY, float roboPosTheta, double roboX, double roboY, double roboZ, double *wldX, double *wldY, double *wldZ)
{
	double theta = roboPosTheta*PI/180;
	*wldX = roboX*cos(theta) - roboY*sin(theta) + roboPosX;
	*wldY = roboX*sin(theta) + roboY*cos(theta) + roboPosY;
	*wldZ = roboZ;
}

void CCoordinateTrans::Camera2World(float roboPosX, float roboPosY, float roboPosTheta, float pan, float tilt, double camX, double camY, double camZ, double *wldX, double *wldY, double *wldZ, bool flag)
{
	double roboX, roboY, roboZ;

	//カメラ座標⇒ロボット座標
	Camera2Robot(pan, tilt, camX, camY, camZ, &roboX, &roboY, &roboZ, flag);

	//ロボット座標⇒ワールド座標
	Robot2World(roboPosX, roboPosY, roboPosTheta, roboX, roboY, roboZ, wldX, wldY, wldZ);
}

void CCoordinateTrans::Robot2LeftArm(double roboX, double roboY, double roboZ, double *armX, double *armY, double *armZ)
{
	//平行移動だけ考える
	*armX = roboX - LEFTARM_X;
	*armY = roboY - LEFTARM_Y;
	*armZ = roboZ - LEFTARM_Z;
}

void CCoordinateTrans::Robot2RightArm(double roboX, double roboY, double roboZ, double *armX, double *armY, double *armZ)
{
	//平行移動だけ考える
	*armX = roboX - RIGHTARM_X;
	*armY = roboY - RIGHTARM_Y;
	*armZ = roboZ - RIGHTARM_Z;
}

void CCoordinateTrans::LeftArm2LeftHand(float joint[6], double armX, double armY, double armZ, double *handX, double *handY, double *handZ, bool flag)
{
	//アーム座標代入
	cvmSet(m_Parm, 0, 0, armX);
	cvmSet(m_Parm, 1, 0, armY);
	cvmSet(m_Parm, 2, 0, armZ);
	cvmSet(m_Parm, 3, 0, 1);

	if(flag)
	{
		//同次変換行列作成
		CalcTransMatrix(m_Th_1,	           0,					   0,		       0,	-joint[5]*PI/180);
		CalcTransMatrix(m_T1_2,	           0,		-joint[4]*PI/180,		       0,				   0);
		CalcTransMatrix(m_T2_3,            0,					   0,	-ARM_LINK_L4,	-joint[3]*PI/180);
		CalcTransMatrix(m_T3_4,            0,	(joint[2]-90)*PI/180,	-ARM_LINK_L2,				   0);
		CalcTransMatrix(m_T4_5, -ARM_LINK_L3,	(joint[1]-90)*PI/180,			   0,			   -PI/2);
		CalcTransMatrix(m_T5_a,            0,					   0,			   0,	-joint[0]*PI/180);

		//アーム⇒ハンドの同次変換行列
		cvmMul( m_Th_1 , m_T1_2 , m_Th_a );
		cvmMul( m_Th_a , m_T2_3 , m_Th_a );
		cvmMul( m_Th_a , m_T3_4 , m_Th_a );
		cvmMul( m_Th_a , m_T4_5 , m_Th_a );
		cvmMul( m_Th_a , m_T5_a , m_Th_a );
	}
	
	//座標計算
	cvmMul(m_Th_a, m_Parm, m_Phand);

	//計算結果代入
	*handX = cvmGet( m_Phand , 0 , 0 );
	*handY = cvmGet( m_Phand , 1 , 0 );
	*handZ = cvmGet( m_Phand , 2 , 0 );
}
/*
void CCoordinateTrans::LeftArm2LeftHand(float joint[6], double armX, double armY, double armZ, double *handX, double *handY, double *handZ, bool flag)
{
	//アーム座標代入
	cvmSet(m_Parm, 0, 0, armX);
	cvmSet(m_Parm, 1, 0, armY);
	cvmSet(m_Parm, 2, 0, armZ);
	cvmSet(m_Parm, 3, 0, 1);

	if(flag)
	{
		//同次変換行列作成
		//CalcTransMatrix(m_Th_1,	           0,					   0,		       0,	-joint[5]*PI/180);
		//CalcTransMatrix(m_T1_2,	           0,		-joint[4]*PI/180,		       0,				   0);
		//CalcTransMatrix(m_T2_3,            0,					   0,	-ARM_LINK_L4,	-joint[3]*PI/180);
		CalcTransMatrix(m_T3_4,            0,					   0,	-ARM_LINK_L2,				   0);
		CalcTransMatrix(m_T4_5, -ARM_LINK_L3,	(joint[1]-90)*PI/180,			   0,			   -PI/2);
		CalcTransMatrix(m_T5_a,            0,					   0,			   0,	-joint[0]*PI/180);

		//アーム⇒ハンドの同次変換行列
	//	cvmMul( m_Th_1 , m_T1_2 , m_Th_a );
	//	cvmMul( m_Th_a , m_T2_3 , m_Th_a );
	//	cvmMul( m_Th_a , m_T3_4 , m_Th_a );
		cvmMul( m_T3_4 , m_T4_5 , m_Th_a );
		cvmMul( m_Th_a , m_T5_a , m_Th_a );
	}
	
	//座標計算
	cvmMul(m_Th_a, m_Parm, m_Phand);

	//計算結果代入
	*handX = cvmGet( m_Phand , 0 , 0 );
	*handY = cvmGet( m_Phand , 1 , 0 );
	*handZ = cvmGet( m_Phand , 2 , 0 );
}*/

void CCoordinateTrans::RightArm2RightHand(float joint[6], double armX, double armY, double armZ, double *handX, double *handY, double *handZ, bool flag)
{
	//アーム座標代入
	cvmSet(m_Parm, 0, 0, armX);
	cvmSet(m_Parm, 1, 0, armY);
	cvmSet(m_Parm, 2, 0, armZ);
	cvmSet(m_Parm, 3, 0, 1);

	if(flag)
	{
		//同次変換行列作成
		CalcTransMatrix(m_Th_1,	           0,					   0,		       0,	-joint[5]*PI/180);
		CalcTransMatrix(m_T1_2,	           0,		-joint[4]*PI/180,		       0,				   0);
		CalcTransMatrix(m_T2_3,            0,					   0,	-ARM_LINK_L4,	-joint[3]*PI/180);
		CalcTransMatrix(m_T3_4,            0,	(joint[2]-90)*PI/180,	-ARM_LINK_L2,				   0);
		CalcTransMatrix(m_T4_5, -ARM_LINK_L3,	(joint[1]-90)*PI/180,	-ARM_LINK_L1,			    PI/2);
		CalcTransMatrix(m_T5_a,            0,					   0,			   0,	-joint[0]*PI/180);

		//アーム⇒ハンドの同次変換行列
		cvmMul( m_Th_1 , m_T1_2 , m_Th_a );
		cvmMul( m_Th_a , m_T2_3 , m_Th_a );
		cvmMul( m_Th_a , m_T3_4 , m_Th_a );
		cvmMul( m_Th_a , m_T4_5 , m_Th_a );
		cvmMul( m_Th_a , m_T5_a , m_Th_a );
	}
	
	//座標計算
	cvmMul(m_Th_a, m_Parm, m_Phand);

	//計算結果代入
	*handX = cvmGet( m_Phand , 0 , 0 );
	*handY = cvmGet( m_Phand , 1 , 0 );
	*handZ = cvmGet( m_Phand , 2 , 0 );
}

void CCoordinateTrans::Camera2LeftHand(float pan, float tilt, float joint[6], double camX, double camY, double camZ, double *handX, double *handY, double *handZ, bool flag)
{
	double roboX, roboY, roboZ;
	double armX, armY, armZ;

	//カメラ座標⇒ロボット座標
	Camera2Robot(pan, tilt, camX, camY, camZ, &roboX, &roboY, &roboZ, flag);

	//ロボット座標⇒アーム座標
	Robot2LeftArm(roboX, roboY, roboZ, &armX, &armY, &armZ);

	//アーム座標⇒ハンド座標
	LeftArm2LeftHand(joint, armX, armY, armZ, handX, handY, handZ, flag);
}

void CCoordinateTrans::Camera2RightHand(float pan, float tilt, float joint[6], double camX, double camY, double camZ, double *handX, double *handY, double *handZ, bool flag)
{
	double roboX, roboY, roboZ;
	double armX, armY, armZ;

	//カメラ座標⇒ロボット座標
	Camera2Robot(pan, tilt, camX, camY, camZ, &roboX, &roboY, &roboZ, flag);

	//ロボット座標⇒アーム座標
	Robot2RightArm(roboX, roboY, roboZ, &armX, &armY, &armZ);

	//アーム座標⇒ハンド座標
	RightArm2RightHand(joint, armX, armY, armZ, handX, handY, handZ, flag);
}

void CCoordinateTrans::CalcTransMatrix( CvMat *trans , double a , double alpha , double d , double theta )
{
	cvmSet( trans , 0 , 0 , cos(theta) );
	cvmSet( trans , 0 , 1 , -sin(theta) );
	cvmSet( trans , 0 , 2 , 0 );
	cvmSet( trans , 0 , 3 , a );

	cvmSet( trans , 1 , 0 , cos(alpha)*sin(theta) );
	cvmSet( trans , 1 , 1 , cos(alpha)*cos(theta) );
	cvmSet( trans , 1 , 2 , -sin(alpha) );
	cvmSet( trans , 1 , 3 , -d*sin(alpha) );

	cvmSet( trans , 2 , 0 , sin(alpha)*sin(theta) );
	cvmSet( trans , 2 , 1 , sin(alpha)*cos(theta) );
	cvmSet( trans , 2 , 2 , cos(alpha) );
	cvmSet( trans , 2 , 3 , d*cos(alpha) );
	
	cvmSet( trans , 3 , 0 , 0 );
	cvmSet( trans , 3 , 1 , 0 );
	cvmSet( trans , 3 , 2 , 0 );
	cvmSet( trans , 3 , 3 , 1 );
}

void CCoordinateTrans::CalcParallelShiftMatrix( CvMat *trans , double x , double y , double z )
{
	cvZero( trans );
	
	//回転行列成分は単位行列
	cvmSet( trans , 0 , 0 , 1 );
	cvmSet( trans , 1 , 1 , 1 );
	cvmSet( trans , 2 , 2 , 1 );
	
	//平行移動成分
	cvmSet( trans , 0 , 3 , x );
	cvmSet( trans , 1 , 3 , y );
	cvmSet( trans , 2 , 3 , z );
	cvmSet( trans , 3 , 3 , 1 );
}