#include "../stdafx.h"
#include "ArmCollision.h"
#include "OpenCV.h"
#include <math.h>

ArmCollision::ArmCollision()
{
	//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_AdjustY1_b = cvCreateMat( 3 , 1 , CV_64F );
	m_AdjustY2_b = cvCreateMat( 3 , 1 , CV_64F );
	m_AdjustY1_a = cvCreateMat( 3 , 1 , CV_64F );
	m_AdjustY2_a = 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 );


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

	cvmSet(m_AdjustY1_b, 0, 0,   0);
	cvmSet(m_AdjustY1_b, 1, 0, 170);
	cvmSet(m_AdjustY1_b, 2, 0,   0);

	cvmSet(m_AdjustY2_b, 0, 0,   0);
	cvmSet(m_AdjustY2_b, 1, 0, -70);
	cvmSet(m_AdjustY2_b, 2, 0,   0);

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

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

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

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

	// 第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, 160);
}

ArmCollision::~ArmCollision()
{
	//
	// 解放
	//

	cvReleaseMat(&m_AdjustY1_b);
	cvReleaseMat(&m_AdjustY2_b);
	cvReleaseMat(&m_AdjustY1_a);
	cvReleaseMat(&m_AdjustY2_a);

	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 ArmCollision::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;
	double dist;
	double radian90 = 90.0*M_PI/180.0;

	try
	{
		// 第6関節
		cvSub(m_Pa, m_Link5_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_Link6_r_after, 0, 0);
		y = cvmGet(m_Link6_r_after, 1, 0);
		z = cvmGet(m_Link6_r_after, 2, 0);

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

		if ( dist * sin(theta) < 90 && 
			 dist * cos(theta) < 170 &&
			 theta < radian90
		   )
		{
			m_count[5]++;
			throw true;
		}

		// 第5関節
		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);

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

		if ( dist * sin(theta) < 90  && 
			 dist * cos(theta) < 50 &&
			 theta < radian90
		   )
		{
			m_count[4]++;
			throw true;
		}

		// 第4関節
		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);

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

		if ( dist * sin(theta) < 90  && 
			 dist * cos(theta) < 150 &&
			 theta < radian90
		   )
		{
			m_count[3]++;
			throw true;
		}

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

		Prx = cvmGet(m_tmpVector, 0, 0) -  cvmGet(m_AdjustY2_a, 0, 0);
		Pry = cvmGet(m_tmpVector, 1, 0) -  cvmGet(m_AdjustY2_a, 1, 0);
		Prz = cvmGet(m_tmpVector, 2, 0) -  cvmGet(m_AdjustY2_a, 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);

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

		if ( dist * sin(theta) < 70  && 
			 dist * cos(theta) < 160 &&
			 theta < radian90
		   )
		{
			m_count[2]++;
			throw true;
		}

		// 第2関節
		/*cvSub(m_Pa, m_Link1_r_before, m_tmpVector); // 相対ベクトルへ変換

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

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

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

		if ( dist * sin(theta) < 90  && 
			 dist * cos(theta) < 420 &&
			 theta < radian90
		   )
		{
			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 ArmCollision::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)*(-1)*(Angle[4])                         );
	cvmSetRotationMatrixZ(m_R6,      (Angle[5] - 90.0*acos(-1.0)/180.0) );

	//for ( int i=0;i<6;i++ ) printf("Angle[%d] = %lf\n",i,Angle[i]*180/M_PI);

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

// 使用する回転行列を生成
void ArmCollision::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 ArmCollision::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);

	cvmMul(m_R1,     m_AdjustY1_b, m_AdjustY1_a);
	cvmMul(m_R2_all, m_AdjustY2_b, m_AdjustY2_a);

	cvmAdd(m_Link1_r_before, m_Link2_r_after, m_Link2_a_after);
	cvmAdd(m_Link2_a_after,  m_AdjustY1_a,    m_Link2_a_after);
	cvmAdd(m_Link2_a_after,  m_Link3_r_after, m_Link3_a_after);
	cvmAdd(m_Link3_a_after,  m_AdjustY2_a,    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 ArmCollision::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 ArmCollision::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);
}
