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

ArmCollision::ArmCollision()
{
	//
	// 生成
	//
	// 計算用
	m_JudgPos2  = cvCreateMat( 3 , 1 , CV_64F );
	m_JudgPos3  = cvCreateMat( 3 , 1 , CV_64F );
	m_JudgPos4  = cvCreateMat( 3 , 1 , CV_64F );
	m_JudgPos5  = cvCreateMat( 3 , 1 , CV_64F );
	m_JudgPos6  = cvCreateMat( 3 , 1 , CV_64F );
	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 );

	m_R1_reverse = cvCreateMat( 3 , 3 , CV_64F );
	m_R2_reverse = cvCreateMat( 3 , 3 , CV_64F );
	m_R3_reverse = cvCreateMat( 3 , 3 , CV_64F );
	m_R4_reverse = cvCreateMat( 3 , 3 , CV_64F );
	m_R5_reverse = cvCreateMat( 3 , 3 , CV_64F );
	m_R6_reverse = cvCreateMat( 3 , 3 , CV_64F );

	m_R2_reverse_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R3_reverse_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R4_reverse_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R5_reverse_all = cvCreateMat( 3 , 3 , CV_64F );
	m_R6_reverse_all = cvCreateMat( 3 , 3 , CV_64F );


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

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

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

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

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

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

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

ArmCollision::~ArmCollision()
{
	//
	// 解放
	//
	cvReleaseMat(&m_JudgPos2);
	cvReleaseMat(&m_JudgPos3);
	cvReleaseMat(&m_JudgPos4);
	cvReleaseMat(&m_JudgPos5);
	cvReleaseMat(&m_JudgPos6);
	cvReleaseMat(&m_tmpVector);
	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);

	cvReleaseMat(&m_R1_reverse);
	cvReleaseMat(&m_R2_reverse);
	cvReleaseMat(&m_R3_reverse);
	cvReleaseMat(&m_R4_reverse);
	cvReleaseMat(&m_R5_reverse);
	cvReleaseMat(&m_R6_reverse);

	cvReleaseMat(&m_R2_reverse_all);
	cvReleaseMat(&m_R3_reverse_all);
	cvReleaseMat(&m_R4_reverse_all);
	cvReleaseMat(&m_R5_reverse_all);
	cvReleaseMat(&m_R6_reverse_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;

	try
	{
		// 第6関節
		cvSub(m_Pa, m_Link5_a_after, m_tmpVector);          // 相対ベクトルへ変換
		cvmMul(m_R6_reverse_all, m_tmpVector, m_tmpVector); // 判定する座標を基準へ戻す方向へ回転させる
		cvmAdd(m_tmpVector, m_Link5_a_before, m_JudgPos6);  // 絶対ベクトルへ変換
		x = cvmGet(m_JudgPos6, 0, 0);
		y = cvmGet(m_JudgPos6, 1, 0);
		z = cvmGet(m_JudgPos6, 2, 0);

		if ( x > ARM5X_MIN && x < ARM5X_MAX &&
			 y > ARM5Y_MIN && y < ARM5Y_MAX &&
			 z > ARM5Z_MIN && z < ARM5Z_MAX    )
		{
			throw true;
		}

		// 第5関節
		cvSub(m_Pa, m_Link4_a_after, m_tmpVector);          // 相対ベクトルへ変換
		cvmMul(m_R5_reverse_all, m_tmpVector, m_tmpVector); // 判定する座標を基準へ戻す方向へ回転させる
		cvmAdd(m_tmpVector, m_Link4_a_before, m_JudgPos5);  // 絶対ベクトルへ変換
		x = cvmGet(m_JudgPos5, 0, 0);
		y = cvmGet(m_JudgPos5, 1, 0);
		z = cvmGet(m_JudgPos5, 2, 0);

		if ( x > ARM4X_MIN && x < ARM4X_MAX &&
			 y > ARM4Y_MIN && y < ARM4Y_MAX &&
			 z > ARM4Z_MIN && z < ARM4Z_MAX    )
		{
			throw true;
		}

		// 第4関節
		cvSub(m_Pa, m_Link3_a_after, m_tmpVector);          // 相対ベクトルへ変換
		cvmMul(m_R4_reverse_all, m_tmpVector, m_tmpVector); // 判定する座標を基準へ戻す方向へ回転させる
		cvmAdd(m_tmpVector, m_Link3_a_before, m_JudgPos4);  // 絶対ベクトルへ変換
		x = cvmGet(m_JudgPos4, 0, 0);
		y = cvmGet(m_JudgPos4, 1, 0);
		z = cvmGet(m_JudgPos4, 2, 0);

		if ( x > ARM3X_MIN && x < ARM3X_MAX &&
			 y > ARM3Y_MIN && y < ARM3Y_MAX &&
			 z > ARM3Z_MIN && z < ARM3Z_MAX    )
		{
			throw true;
		}

		// 第3関節
		cvSub(m_Pa, m_Link2_a_after, m_tmpVector);          // 相対ベクトルへ変換
		x = cvmGet(m_Link2_a_after, 0, 0);
		y = cvmGet(m_Link2_a_after, 1, 0);
		z = cvmGet(m_Link2_a_after, 2, 0);
		cvmMul(m_R3_reverse_all, m_tmpVector, m_tmpVector); // 判定する座標を基準へ戻す方向へ回転させる
		cvmAdd(m_tmpVector, m_Link2_a_before, m_JudgPos3);  // 絶対ベクトルへ変換
		x = cvmGet(m_JudgPos3, 0, 0);
		y = cvmGet(m_JudgPos3, 1, 0);
		z = cvmGet(m_JudgPos3, 2, 0);

		if ( x > ARM2X_MIN && x < ARM2X_MAX &&
			 y > ARM2Y_MIN && y < ARM2Y_MAX &&
			 z > ARM2Z_MIN && z < ARM2Z_MAX    )
		{
			throw true;
		}

		// 第2関節
		cvSub(m_Pa, m_Link1_r_before, m_tmpVector);         // 相対ベクトルへ変換
		cvmMul(m_R2_reverse_all, m_tmpVector, m_tmpVector); // 判定する座標を基準へ戻す方向へ回転させる
		cvmAdd(m_tmpVector, m_Link1_r_before, m_JudgPos2);  // 絶対ベクトルへ変換
		x = cvmGet(m_JudgPos2, 0, 0);
		y = cvmGet(m_JudgPos2, 1, 0);
		z = cvmGet(m_JudgPos2, 2, 0);

		if ( x > ARM1X_MIN && x < ARM1X_MAX &&
			 y > ARM1Y_MIN && y < ARM1Y_MAX &&
			 z > ARM1Z_MIN && z < ARM1Z_MAX    )
		{
			throw true;
		}

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

		if ( x > ARM0X_MIN && x < ARM0X_MAX &&
			 y > ARM0Y_MIN && y < ARM0Y_MAX &&
			 z > ARM0Z_MIN && z < ARM0Z_MAX    )
		{
			throw true;
		}

		// 衝突しない
		throw false;
	}
	catch( bool bCollision )
	{
		return bCollision;
	}
}

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

	// 衝突判定座標を基準に戻す方向への回転行列を求める
	cvmSetRotationMatrixZ(m_R1_reverse, (-1)*(Angle[0])                         );
	cvmSetRotationMatrixY(m_R2_reverse, (-1)*(Angle[1] - 90.0*acos(-1.0)/180.0) );
	cvmSetRotationMatrixY(m_R3_reverse, (-1)*(Angle[2] - 90.0*acos(-1.0)/180.0) );
	cvmSetRotationMatrixZ(m_R4_reverse, (-1)*(Angle[3])                         );
	cvmSetRotationMatrixY(m_R5_reverse, (-1)*(Angle[4])                         );
	cvmSetRotationMatrixZ(m_R6_reverse, (-1)*(Angle[5]- 90.0*acos(-1.0)/180.0)  );

	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);

	cvmMul(m_R1_reverse,     m_R2_reverse, m_R2_reverse_all);
	cvmMul(m_R2_reverse_all, m_R3_reverse, m_R3_reverse_all);
	cvmMul(m_R3_reverse_all, m_R4_reverse, m_R4_reverse_all);
	cvmMul(m_R4_reverse_all, m_R5_reverse, m_R5_reverse_all);
	cvmMul(m_R5_reverse_all, m_R6_reverse, m_R6_reverse_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);

	cvmAdd(m_Link1_r_before, m_Link2_r_before, m_Link2_a_before);
	cvmAdd(m_Link2_a_before, m_Link3_r_before, m_Link3_a_before);
	cvmAdd(m_Link3_a_before, m_Link4_r_before, m_Link4_a_before);
	cvmAdd(m_Link4_a_before, m_Link5_r_before, m_Link5_a_before);
	cvmAdd(m_Link5_a_before, m_Link6_r_before, m_Link6_a_before);

	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 ArmCollision::cvmSetRotationMatrixY(CvMat *rot, double angle)
{
	angle *= (-1); // 何故？？

	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)
{
	//angle *= (-1); // 何故？？

	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);
}
