#include "stdafx.h"
#include "ArmObstaclize.h"
#include "OpenCV.h"
#include <math.h>

ArmObstaclize::ArmObstaclize()
{
	//debug
	for ( int i=0;i<6;i++ ) m_count[i] = 0;
	//
	// 生成
	//
	// 計算用
	m_tmpVector = cvCreateMat( 3 , 1 , CV_64F );
	m_Pa        = cvCreateMat( 3 , 1 , CV_64F );

	// リンクベクトル
	m_Link1_r_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link2_r_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link3_r_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link4_r_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link5_r_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link6_r_before = cvCreateMat( 3 , 1 , CV_64F );

	m_Link2_a_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link3_a_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link4_a_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link5_a_before = cvCreateMat( 3 , 1 , CV_64F );
	m_Link6_a_before = cvCreateMat( 3 , 1 , CV_64F );

	m_Link1_r_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link2_r_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link3_r_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link4_r_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link5_r_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link6_r_after = cvCreateMat( 3 , 1 , CV_64F );

	m_Link2_a_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link3_a_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link4_a_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link5_a_after = cvCreateMat( 3 , 1 , CV_64F );
	m_Link6_a_after = cvCreateMat( 3 , 1 , CV_64F );

	// 回転行列
	m_R1 = cvCreateMat( 3 , 3 , CV_64F );
	m_R2 = cvCreateMat( 3 , 3 , CV_64F );
	m_R3 = cvCreateMat( 3 , 3 , CV_64F );
	m_R4 = cvCreateMat( 3 , 3 , CV_64F );
	m_R5 = cvCreateMat( 3 , 3 , CV_64F );
	m_R6 = cvCreateMat( 3 , 3 , CV_64F );

	m_R2_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R3_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R4_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R5_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R6_all = cvCreateMat( 3 , 3 , CV_64F );


	//
	// セット
	//
	// 各関節の初期ベクトル(相対)

	// 第1関節リンクベクトル
	cvmSet(m_Link1_r_before, 0, 0, 0);
	cvmSet(m_Link1_r_before, 1, 0, 180);
	cvmSet(m_Link1_r_before, 2, 0, 420);

	// 第2関節リンクベクトル
	cvmSet(m_Link2_r_before, 0, 0, 0);
	cvmSet(m_Link2_r_before, 1, 0, -60);
	cvmSet(m_Link2_r_before, 2, 0, 405);

	// 第3関節リンクベクトル
	cvmSet(m_Link3_r_before, 0, 0, 0);
	cvmSet(m_Link3_r_before, 1, 0, 0);
	cvmSet(m_Link3_r_before, 2, 0, 75);

	// 第4関節リンクベクトル
	cvmSet(m_Link4_r_before, 0, 0, 0);
	cvmSet(m_Link4_r_before, 1, 0, 0);
	cvmSet(m_Link4_r_before, 2, 0, 125);

	// 第5関節リンクベクトル
	cvmSet(m_Link5_r_before, 0, 0, 0);
	cvmSet(m_Link5_r_before, 1, 0, 0);
	cvmSet(m_Link5_r_before, 2, 0, 45);

	// 第6関節リンクベクトル
	cvmSet(m_Link6_r_before, 0, 0, 0);
	cvmSet(m_Link6_r_before, 1, 0, 0);
	cvmSet(m_Link6_r_before, 2, 0, 150);
}

ArmObstaclize::~ArmObstaclize()
{
	//
	// 解放
	//
	cvReleaseMat(&m_Pa);

	cvReleaseMat(&m_Link1_r_before);
	cvReleaseMat(&m_Link2_r_before);
	cvReleaseMat(&m_Link3_r_before);
	cvReleaseMat(&m_Link4_r_before);
	cvReleaseMat(&m_Link5_r_before);
	cvReleaseMat(&m_Link6_r_before);

	cvReleaseMat(&m_Link2_a_before);
	cvReleaseMat(&m_Link3_a_before);
	cvReleaseMat(&m_Link4_a_before);
	cvReleaseMat(&m_Link5_a_before);
	cvReleaseMat(&m_Link6_a_before);

	cvReleaseMat(&m_Link1_r_after);
	cvReleaseMat(&m_Link2_r_after);
	cvReleaseMat(&m_Link3_r_after);
	cvReleaseMat(&m_Link4_r_after);
	cvReleaseMat(&m_Link5_r_after);
	cvReleaseMat(&m_Link6_r_after);

	cvReleaseMat(&m_Link2_a_after);
	cvReleaseMat(&m_Link3_a_after);
	cvReleaseMat(&m_Link4_a_after);
	cvReleaseMat(&m_Link5_a_after);
	cvReleaseMat(&m_Link6_a_after);

	cvReleaseMat(&m_R1);
	cvReleaseMat(&m_R2);
	cvReleaseMat(&m_R3);
	cvReleaseMat(&m_R4);
	cvReleaseMat(&m_R5);
	cvReleaseMat(&m_R6);

	cvReleaseMat(&m_R2_all);
	cvReleaseMat(&m_R3_all);
	cvReleaseMat(&m_R4_all);
	cvReleaseMat(&m_R5_all);
	cvReleaseMat(&m_R6_all);
}

bool ArmObstaclize::isCollision(double P[3], bool isLeftArm)
{
	if ( isLeftArm ) 
	{
		// 判定座標を左手の基準座標へ変換
		cvmSet(m_Pa, 0, 0, P[0] - ARM_ORIGIN_X);
		cvmSet(m_Pa, 1, 0, P[1] - ARM_ORIGIN_Y);
		cvmSet(m_Pa, 2, 0, P[2] - ARM_ORIGIN_Z);
	}
	else
	{
		// 判定座標を右手の基準座標へ変換
		cvmSet(m_Pa, 0, 0, P[0] - ARM_ORIGIN_X);
		cvmSet(m_Pa, 1, 0, P[1] + ARM_ORIGIN_Y);
		cvmSet(m_Pa, 2, 0, P[2] - ARM_ORIGIN_Z);
	}

	double x,y,z;
	double Prx,Pry,Prz;
	double cosine_theta;
	double theta;

	try
	{
		// 第6関節
		/*x = cvmGet(m_Link5_a_after, 0, 0);
		y = cvmGet(m_Link5_a_after, 1, 0);
		z = cvmGet(m_Link5_a_after, 2, 0);
		printf("-----回転行列アーム座標-----\n");
		printf("m_Link5_a_afterx = %lf\n",x);
		printf("m_Link5_a_aftery = %lf\n",y);
		printf("m_Link5_a_afterz = %lf\n\n",z);

		cvSub(m_Pa, m_Link5_a_after, m_tmpVector); // 相対ベクトルへ変換

		x = cvmGet(m_Pa, 0, 0);
		y = cvmGet(m_Pa, 1, 0);
		z = cvmGet(m_Pa, 2, 0);
		printf("-----運動学アーム座標-----\n");
		printf("m_Pax = %lf\n",x);
		printf("m_Pay = %lf\n",y);
		printf("m_Paz = %lf\n\n",z);

		x = cvmGet(m_tmpVector, 0, 0);
		y = cvmGet(m_tmpVector, 1, 0);
		z = cvmGet(m_tmpVector, 2, 0);
		printf("-----運動学相対ベクトル-----\n");
		printf("m_tmpVectorx = %lf\n",x);
		printf("m_tmpVectory = %lf\n",y);
		printf("m_tmpVectorz = %lf\n\n",z);*/
		/////////////////////////////////////////////////////////////////////////////////

		Prx = cvmGet(m_tmpVector, 0, 0);
		Pry = cvmGet(m_tmpVector, 1, 0) ;
		Prz = cvmGet(m_tmpVector, 2, 0);

		x = cvmGet(m_Link6_r_after, 0, 0);
		y = cvmGet(m_Link6_r_after, 1, 0);
		z = cvmGet(m_Link6_r_after, 2, 0);

		/*printf("-----各成分の差-----\n");
		printf("dx = %lf\n",fabs(x-Prx));
		printf("dy = %lf\n",fabs(y-Pry));
		printf("dz = %lf\n\n",fabs(z-Prz));*/

		cosine_theta = ( (Prx*x)+(Pry*y)+(Prz*z) ) / ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sqrt(x*x+y*y+z*z) );
		theta = fabs( acos(cosine_theta) ); // それぞれのベクトルとのなす角の絶対値

		/*printf("-----回転行列相対ベクトル-----\n");
		printf("link6x = %lf\n",x);
		printf("link6y = %lf\n",y);
		printf("link6z = %lf\n\n",z);

		printf("-----結果-----\n");
		printf("横距離 = %lf\n",sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sin(theta));
		printf("theta = %lf\n",theta*180.0/M_PI);
		printf("1つ前のリンクからの距離 = %lf\n\n",sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * cos(theta));*/

		if ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sin(theta) < 140  && 
			 sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * cos(theta) < 270 &&
			 theta < 90*M_PI/180.0 
		   )
		{
			m_count[5]++;
			throw true;
		}

		// 第5関節
		x = cvmGet(m_Link4_a_after, 0, 0);
		y = cvmGet(m_Link4_a_after, 1, 0);
		z = cvmGet(m_Link4_a_after, 2, 0);

		cvSub(m_Pa, m_Link4_a_after, m_tmpVector); // 相対ベクトルへ変換

		Prx = cvmGet(m_tmpVector, 0, 0);
		Pry = cvmGet(m_tmpVector, 1, 0) ;
		Prz = cvmGet(m_tmpVector, 2, 0);

		x = cvmGet(m_Link5_r_after, 0, 0);
		y = cvmGet(m_Link5_r_after, 1, 0);
		z = cvmGet(m_Link5_r_after, 2, 0);

		cosine_theta = ( (Prx*x)+(Pry*y)+(Prz*z) ) / ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sqrt(x*x+y*y+z*z) );
		theta = fabs( acos(cosine_theta) ); // それぞれのベクトルとのなす角の絶対値

		if ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sin(theta) < 120  && 
			 sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * cos(theta) < 80 &&
			 theta < 90*M_PI/180.0 
		   )
		{
			m_count[4]++;
			throw true;
		}

		// 第4関節
		x = cvmGet(m_Link3_a_after, 0, 0);
		y = cvmGet(m_Link3_a_after, 1, 0);
		z = cvmGet(m_Link3_a_after, 2, 0);

		cvSub(m_Pa, m_Link3_a_after, m_tmpVector); // 相対ベクトルへ変換

		Prx = cvmGet(m_tmpVector, 0, 0);
		Pry = cvmGet(m_tmpVector, 1, 0) ;
		Prz = cvmGet(m_tmpVector, 2, 0);

		x = cvmGet(m_Link4_r_after, 0, 0);
		y = cvmGet(m_Link4_r_after, 1, 0);
		z = cvmGet(m_Link4_r_after, 2, 0);

		cosine_theta = ( (Prx*x)+(Pry*y)+(Prz*z) ) / ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sqrt(x*x+y*y+z*z) );
		theta = fabs( acos(cosine_theta) ); // それぞれのベクトルとのなす角の絶対値

		if ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sin(theta) < 120  && 
			 sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * cos(theta) < 180 &&
			 theta < 90*M_PI/180.0 
		   )
		{
			m_count[3]++;
			throw true;
		}

		// 第3関節
		x = cvmGet(m_Link2_a_after, 0, 0);
		y = cvmGet(m_Link2_a_after, 1, 0);
		z = cvmGet(m_Link2_a_after, 2, 0);

		cvSub(m_Pa, m_Link2_a_after, m_tmpVector); // 相対ベクトルへ変換

		Prx = cvmGet(m_tmpVector, 0, 0);
		Pry = cvmGet(m_tmpVector, 1, 0) ;
		Prz = cvmGet(m_tmpVector, 2, 0);

		x = cvmGet(m_Link3_r_after, 0, 0);
		y = cvmGet(m_Link3_r_after, 1, 0);
		z = cvmGet(m_Link3_r_after, 2, 0);

		cosine_theta = ( (Prx*x)+(Pry*y)+(Prz*z) ) / ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sqrt(x*x+y*y+z*z) );
		theta = fabs( acos(cosine_theta) ); // それぞれのベクトルとのなす角の絶対値

		if ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sin(theta) < 80  && 
			 sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * cos(theta) < 100 &&
			 theta < 90*M_PI/180.0 
		   )
		{
			m_count[2]++;
			throw true;
		}

		// 第2関節
		x = cvmGet(m_Link3_a_after, 0, 0);
		y = cvmGet(m_Link3_a_after, 1, 0);
		z = cvmGet(m_Link3_a_after, 2, 0);

		cvSub(m_Pa, m_Link3_a_after, m_tmpVector); // 相対ベクトルへ変換

		Prx = cvmGet(m_tmpVector, 0, 0);
		Pry = cvmGet(m_tmpVector, 1, 0) ;
		Prz = cvmGet(m_tmpVector, 2, 0);

		x = cvmGet(m_Link4_r_after, 0, 0);
		y = cvmGet(m_Link4_r_after, 1, 0);
		z = cvmGet(m_Link4_r_after, 2, 0);

		cosine_theta = ( (Prx*x)+(Pry*y)+(Prz*z) ) / ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sqrt(x*x+y*y+z*z) );
		theta = fabs( acos(cosine_theta) ); // それぞれのベクトルとのなす角の絶対値

		if ( sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * sin(theta) < 80  && 
			 sqrt(Prx*Prx+Pry*Pry+Prz*Prz) * cos(theta) < 430 &&
			 theta < 90*M_PI/180.0 
		   )
		{
			m_count[1]++;
			throw true;
		}

		// 第1関節
		x = cvmGet(m_Pa, 0, 0);
		y = cvmGet(m_Pa, 1, 0);
		z = cvmGet(m_Pa, 2, 0);

		if ( x > -100 && x < 100 &&
			 y > -100 && y < 100 &&
			 z > 0    && z < 450    )
		{
			m_count[0]++;
			throw true;
		}

		// 衝突しない
		throw false;

	}
	catch( bool bCollision )
	{
		return bCollision;
	}
}

void ArmObstaclize::MakeArmObstacle(vector<double> *v_X, vector<double> *v_Y, vector<double> *v_Z, bool isLeft)
{
	// Robot座標をArm座標系へ変換
	double Robot2Arm[3];
	if ( isLeft ) 
	{
		Robot2Arm[0] =  ARM_ORIGIN_X;
		Robot2Arm[1] =  ARM_ORIGIN_Y;
		Robot2Arm[2] =  ARM_ORIGIN_Z;
	}
	else
	{
		Robot2Arm[0] =  ARM_ORIGIN_X;
		Robot2Arm[1] =  (-1)*ARM_ORIGIN_Y;
		Robot2Arm[2] =  ARM_ORIGIN_Z;
	}

	v_X->clear();
	v_Y->clear();
	v_Z->clear();

	double ax,ay,az; // abs
	double rx,ry,rz; // relative
	int num;

	// 第6関節
	ax = cvmGet(m_Link5_a_after, 0, 0) - Robot2Arm[0];
	ay = cvmGet(m_Link5_a_after, 1, 0) - Robot2Arm[0];
	az = cvmGet(m_Link5_a_after, 2, 0) - Robot2Arm[0];

	rx = cvmGet(m_Link6_r_after, 0, 0);
	ry = cvmGet(m_Link6_r_after, 1, 0);
	rz = cvmGet(m_Link6_r_after, 2, 0);

	num = (int)(sqrt( (rx*rx) + (ry*ry) + (rz*rz) ) / 50.0);

	for ( int i=0;i<num+1;i++ )
	{
		// ベクトルの定数倍
		v_X->push_back( ax + rx*i );
		v_Y->push_back( ay + ry*i );
		v_Z->push_back( az + rz*i );
	}

	// 第5関節
	ax = cvmGet(m_Link4_a_after, 0, 0) - Robot2Arm[0];
	ay = cvmGet(m_Link4_a_after, 1, 0) - Robot2Arm[0];
	az = cvmGet(m_Link4_a_after, 2, 0) - Robot2Arm[0];

	rx = cvmGet(m_Link5_r_after, 0, 0);
	ry = cvmGet(m_Link5_r_after, 1, 0);
	rz = cvmGet(m_Link5_r_after, 2, 0);

	num = (int)(sqrt( (rx*rx) + (ry*ry) + (rz*rz) ) / 50.0);

	for ( int i=0;i<num+1;i++ )
	{
		// ベクトルの定数倍
		v_X->push_back( ax + rx*i );
		v_Y->push_back( ay + ry*i );
		v_Z->push_back( az + rz*i );
	}

	// 第4関節
	ax = cvmGet(m_Link3_a_after, 0, 0) - Robot2Arm[0];
	ay = cvmGet(m_Link3_a_after, 1, 0) - Robot2Arm[0];
	az = cvmGet(m_Link3_a_after, 2, 0) - Robot2Arm[0];

	rx = cvmGet(m_Link4_r_after, 0, 0);
	ry = cvmGet(m_Link4_r_after, 1, 0);
	rz = cvmGet(m_Link4_r_after, 2, 0);

	num = (int)(sqrt( (rx*rx) + (ry*ry) + (rz*rz) ) / 50.0);

	for ( int i=0;i<num+1;i++ )
	{
		// ベクトルの定数倍
		v_X->push_back( ax + rx*i );
		v_Y->push_back( ay + ry*i );
		v_Z->push_back( az + rz*i );
	}

	// 第3関節
	ax = cvmGet(m_Link2_a_after, 0, 0) - Robot2Arm[0];
	ay = cvmGet(m_Link2_a_after, 1, 0) - Robot2Arm[0];
	az = cvmGet(m_Link2_a_after, 2, 0) - Robot2Arm[0];

	rx = cvmGet(m_Link3_r_after, 0, 0);
	ry = cvmGet(m_Link3_r_after, 1, 0);
	rz = cvmGet(m_Link3_r_after, 2, 0);

	num = (int)(sqrt( (rx*rx) + (ry*ry) + (rz*rz) ) / 50.0);

	for ( int i=0;i<num+1;i++ )
	{
		// ベクトルの定数倍
		v_X->push_back( ax + rx*i );
		v_Y->push_back( ay + ry*i );
		v_Z->push_back( az + rz*i );
	}

	// 第2関節
	ax = cvmGet(m_Link1_r_after, 0, 0) - Robot2Arm[0];
	ay = cvmGet(m_Link1_r_after, 1, 0) - Robot2Arm[0];
	az = cvmGet(m_Link1_r_after, 2, 0) - Robot2Arm[0];

	rx = cvmGet(m_Link2_r_after, 0, 0);
	ry = cvmGet(m_Link2_r_after, 1, 0);
	rz = cvmGet(m_Link2_r_after, 2, 0);

	num = (int)(sqrt( (rx*rx) + (ry*ry) + (rz*rz) ) / 50.0);

	for ( int i=0;i<num+1;i++ )
	{
		// ベクトルの定数倍
		v_X->push_back( ax + rx*i );
		v_Y->push_back( ay + ry*i );
		v_Z->push_back( az + rz*i );
	}
}

// 角度をセット
void ArmObstaclize::SetAngle(double Angle[6])
{
	// 各関節の回転行列を求める
	cvmSetRotationMatrixZ(m_R1,      (Angle[0])                         );
	cvmSetRotationMatrixY(m_R2, (-1)*(Angle[1] - 90.0*acos(-1.0)/180.0) );
	cvmSetRotationMatrixY(m_R3, (-1)*(Angle[2] - 90.0*acos(-1.0)/180.0) );
	cvmSetRotationMatrixZ(m_R4,      (Angle[3])                         );
	cvmSetRotationMatrixY(m_R5, (-1)*(Angle[4])                         );
	cvmSetRotationMatrixZ(m_R6,      (Angle[5] - 90.0*acos(-1.0)/180.0) );

	CreateRotationMatrixList();   // 回転行列を計算
	CreateRotationMatrixVector(); // ベクトルを計算
}

// 使用する回転行列を生成
void ArmObstaclize::CreateRotationMatrixList()
{
	cvmMul(m_R1,     m_R2, m_R2_all);
	cvmMul(m_R2_all, m_R3, m_R3_all);
	cvmMul(m_R3_all, m_R4, m_R4_all);
	cvmMul(m_R4_all, m_R5, m_R5_all);
	cvmMul(m_R5_all, m_R6, m_R6_all);
}
// 使用するベクトルの生成
void ArmObstaclize::CreateRotationMatrixVector()
{
	cvmMul(m_R2_all, m_Link2_r_before, m_Link2_r_after);
	cvmMul(m_R3_all, m_Link3_r_before, m_Link3_r_after);
	cvmMul(m_R4_all, m_Link4_r_before, m_Link4_r_after); 
	cvmMul(m_R5_all, m_Link5_r_before, m_Link5_r_after);
	cvmMul(m_R6_all, m_Link6_r_before, m_Link6_r_after);

	cvmAdd(m_Link1_r_before, m_Link2_r_after, m_Link2_a_after);
	cvmAdd(m_Link2_a_after,  m_Link3_r_after, m_Link3_a_after);
	cvmAdd(m_Link3_a_after,  m_Link4_r_after, m_Link4_a_after);
	cvmAdd(m_Link4_a_after,  m_Link5_r_after, m_Link5_a_after);
	cvmAdd(m_Link5_a_after,  m_Link6_r_after, m_Link6_a_after);
}

void ArmObstaclize::cvmSetRotationMatrixY(CvMat *rot, double angle)
{
	cvmSet(rot,0,0, cos(angle) );
	cvmSet(rot,0,1, 0 );
	cvmSet(rot,0,2, sin(angle) );

	cvmSet(rot,1,0, 0 );
	cvmSet(rot,1,1, 1 );
	cvmSet(rot,1,2, 0);

	cvmSet(rot,2,0, (-1)*sin(angle) );
	cvmSet(rot,2,1, 0 );
	cvmSet(rot,2,2, cos(angle));
}

void ArmObstaclize::cvmSetRotationMatrixZ(CvMat *rot, double angle)
{
	cvmSet(rot,0,0, cos(angle) );
	cvmSet(rot,0,1, (-1)*sin(angle) );
	cvmSet(rot,0,2, 0);

	cvmSet(rot,1,0, sin(angle) );
	cvmSet(rot,1,1, cos(angle) );
	cvmSet(rot,1,2, 0);

	cvmSet(rot,2,0, 0);
	cvmSet(rot,2,1, 0);
	cvmSet(rot,2,2, 1);
}
