#include "stdafx.h"
#include "ArmSimulaotrODE.h"

bool           ArmSim::m_EndSim;
dWorldID       ArmSim::m_WorldID;
dSpaceID       ArmSim::m_SpaceID;
dJointGroupID  ArmSim::m_contactgroup;
dGeomID        ArmSim::m_ground;
bool           ArmSim::m_isCollision;
double         ArmSim::m_WorldStep;
bool           ArmSim::m_UseGUI;
Object         ArmSim::m_Face[4];
Object         ArmSim::m_base;
Object         ArmSim::m_ArmLeft[ARM_JOINT_NUM];
Object         ArmSim::m_ArmRight[ARM_JOINT_NUM];
Object         ArmSim::m_Object;
int            ArmSim::m_GraspHand;
dJointID       ArmSim::m_ArmLeftJoint[ARM_JOINT_NUM];
dJointID       ArmSim::m_ArmRightJoint[ARM_JOINT_NUM];
dJointID       ArmSim::m_Base2Ground;
dJointID       ArmSim::m_Object2Arm;
dReal          ArmSim::m_VelosityL[ARM_JOINT_NUM];
dReal          ArmSim::m_VelosityR[ARM_JOINT_NUM];
dReal          ArmSim::m_JointAngleL[ARM_JOINT_NUM];
dReal          ArmSim::m_JointAngleR[ARM_JOINT_NUM];
dReal          ArmSim::m_InitAngleL[ARM_JOINT_NUM];
dReal          ArmSim::m_InitAngleR[ARM_JOINT_NUM];
dReal          ArmSim::m_CollisionPos[ARM_JOINT_NUM];
bool           ArmSim::m_Inited;
bool           ArmSim::m_isBalanced;
vector<Object> ArmSim::m_Obstacle;
dReal          ArmSim::m_TargetAngleL[ARM_JOINT_NUM];
dReal          ArmSim::m_TargetAngleR[ARM_JOINT_NUM];
bool           ArmSim::m_UseManual;

ArmSim::ArmSim()
{
	m_fn.version = DS_VERSION;
	m_fn.start = &start;
	m_fn.step = &simLoop;
	m_fn.command = &command;
	m_fn.path_to_textures = "textures";
	m_fn.stop = &stop;

	m_WorldID = dWorldCreate();
	m_SpaceID = dHashSpaceCreate(0);
	m_ground = dCreatePlane(m_SpaceID, 0, 0, 1, 0);
	dWorldSetGravity(m_WorldID, 0, 0, -9.8);

	// 衝突判定時にスーパージャンプを防ぐ　適正値は不明
	dWorldSetContactMaxCorrectingVel(m_WorldID, 0.0001);
	dWorldSetContactSurfaceLayer(m_WorldID, 0.00001);
	//dWorldSetERP(m_WorldID, 1.0);
	//dWorldSetCFM(m_WorldID, 0.0);

	m_GraspHand = GRASP_NOTHING;
	m_isBalanced = false;

	for ( int i=0;i<ARM_JOINT_NUM;i++ )
	{
		m_ArmLeft[i].x  = 0;  m_ArmLeft[i].y  = 0;  m_ArmLeft[i].z  = 0;  m_ArmLeft[i].Rotation  = 0;
		m_ArmRight[i].x = 0;  m_ArmRight[i].y = 0;  m_ArmRight[i].z = 0;  m_ArmRight[i].Rotation = 0;
		m_VelosityL[i] = 0;
		m_VelosityR[i] = 0;
		m_TargetAngleL[i] = 0;
		m_TargetAngleR[i] = 0;
	}

	m_base.r = BASE_R;  m_base.l = BASE_L;  m_base.m = BASE_M;

	m_UseManual = true;

	m_WorldStep = 0.01;
	m_UseGUI = true;
}

ArmSim::~ArmSim()
{
	dGeomDestroy(m_ground);
	dSpaceDestroy(m_SpaceID);
	dWorldDestroy(m_WorldID);
}

// シミュレーション開始
bool ArmSim::StartSimulation()
{
	m_EndSim = false;
	m_contactgroup = false;

	m_contactgroup = dJointGroupCreate(0);

	for ( int i=0;i<ARM_JOINT_NUM;i++ ) m_CollisionPos[i] = 0.0;

	MakeArm();

	m_Inited = false;

	if ( m_UseGUI == true ) dsSimulationLoop(NULL, NULL, 640, 480, &m_fn);
	else                    while ( m_EndSim == false ) { simLoop(0); }

	dBodyDestroy(m_base.bodyID);
	dGeomDestroy(m_base.geomID);
	for ( int i=0;i<ARM_JOINT_NUM;i++ )
	{
		dBodyDestroy(m_ArmLeft[i].bodyID);
		dGeomDestroy(m_ArmLeft[i].geomID);

		dBodyDestroy(m_ArmRight[i].bodyID);
		dGeomDestroy(m_ArmRight[i].geomID);
	}
	for ( int i=0;i<(int)m_Obstacle.size();i++ )
	{
		dBodyDestroy(m_Obstacle[i].bodyID);
		//dGeomDestroy(m_Obstacle[i].geomID);
	}
	for ( int i=0;i<4;i++ )
	{
		dBodyDestroy(m_Face[i].bodyID);
		//dGeomDestroy(m_Face[i].geomID);
	}
	dJointGroupDestroy(m_contactgroup);

	return m_isCollision;
}

// シミュレーション開始前処理
void ArmSim::start()
{
	static float xyz[3] = {1.65,0,1.7};
	static float hpr[3] = {-180,-20,0};
	dsSetViewpoint(xyz, hpr);
}
 // シミュレーション本体
void ArmSim::simLoop(int pause)
{
	if ( m_EndSim == true ) dsStop();

	if ( !pause )
	{
		//if ( m_isBalanced == true ) dSpaceCollide(m_SpaceID, 0, &nearCallback); // 平衡状態の時だけ衝突判定する
		//dSpaceCollide(m_SpaceID, 0, &nearCallback); // 平衡状態の時だけ衝突判定する
		dWorldStep(m_WorldID, m_WorldStep);
		dJointGroupEmpty(m_contactgroup);
	}

	ArmControl();

	if ( m_UseGUI == true && m_Inited == true ) 
	{
		DrawArm();
		DrawObstacle();
	}
}

// キーボード入力
void ArmSim::command(int cmd)
{
}

// シミュレーション終了後処理
void ArmSim::stop()
{
}

// コールバック関数(衝突判定)
void ArmSim::nearCallback(void *data, dGeomID o1, dGeomID o2)
{
	if ( m_Inited == false ) return;

	const int N = 10;
	dContact contact[N];

	dBodyID b1 = dGeomGetBody(o1);
	dBodyID b2 = dGeomGetBody(o2);

	/*
	for (int i=0;i<ARM_JOINT_NUM;i++)
	{
		if      ( b1 == m_ArmLeft[i].bodyID  ) return;
		else if ( b2 == m_ArmLeft[i].bodyID  ) return;
		else if ( b1 == m_ArmRight[i].bodyID ) return;
		else if ( b2 == m_ArmRight[i].bodyID ) return;
	}
	*/
	//if ( b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact) ) return; // 接触点ジョイント以外のジョイント同士ならなら終了

	for (int i=1;i<ARM_JOINT_NUM;i++)
	{
		if      ( b1 == m_ArmLeft[i-1].bodyID && b2  == m_ArmLeft[i].bodyID  ) return;
		else if ( b2 == m_ArmLeft[i-1].bodyID && b1  == m_ArmLeft[i].bodyID  ) return;
		if      ( b1 == m_ArmRight[i-1].bodyID && b2 == m_ArmRight[i].bodyID ) return;
		else if ( b2 == m_ArmRight[i-1].bodyID && b1 == m_ArmRight[i].bodyID ) return;
	}

	if      ( b1 == m_base.bodyID && b2 == m_ArmLeft[0].bodyID  ) return;
	else if ( b2 == m_base.bodyID && b1 == m_ArmLeft[0].bodyID  ) return;
	if      ( b1 == m_base.bodyID && b2 == m_ArmRight[0].bodyID ) return;
	else if ( b2 == m_base.bodyID && b1 == m_ArmRight[0].bodyID ) return;

	for (int i=1;i<ARM_JOINT_NUM-1;i++)
	{
		for ( int j=i+1;j<ARM_JOINT_NUM;j++ )
		{
			if      ( b1 == m_ArmLeft[i].bodyID && b2  == m_ArmLeft[j].bodyID )  return;
			else if ( b2 == m_ArmLeft[i].bodyID && b1  == m_ArmLeft[j].bodyID )  return;
			else if ( b1 == m_ArmRight[i].bodyID && b2 == m_ArmRight[j].bodyID ) return;
			else if ( b2 == m_ArmRight[i].bodyID && b1 == m_ArmRight[j].bodyID ) return;
		}
	}

	if ( b1 == m_Object.bodyID || b2 == m_Object.bodyID ) return;

	bool flag = false;
	int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));

	// アーム自身の衝突
	for (int i=3;i<ARM_JOINT_NUM;i++)
	{
		if      ( b1 == m_ArmLeft[0].bodyID && b2  == m_ArmLeft[i].bodyID  ) flag = true;
		else if ( b2 == m_ArmLeft[0].bodyID && b1  == m_ArmLeft[i].bodyID  ) flag = true;
		if      ( b1 == m_ArmRight[0].bodyID && b2 == m_ArmRight[i].bodyID ) flag = true;
		else if ( b2 == m_ArmRight[0].bodyID && b1 == m_ArmRight[i].bodyID ) flag = true;
	}

	if ( n>0 && flag == true )
	{
		m_isCollision = true;
		//printf("アーム自身の衝突\n");
		//m_EndSim = true;
		return;
	}

	// 土台とアームの衝突
	flag = false;
	for (int i=2;i<ARM_JOINT_NUM;i++)
	{
		if      ( b1 == m_base.bodyID && b2 == m_ArmLeft[i].bodyID  ) flag = true;
		else if ( b2 == m_base.bodyID && b1 == m_ArmLeft[i].bodyID  ) flag = true;
		if      ( b1 == m_base.bodyID && b2 == m_ArmRight[i].bodyID ) flag = true;
		else if ( b2 == m_base.bodyID && b1 == m_ArmRight[i].bodyID ) flag = true;
	}

	if ( n>0 && flag == true )
	{
		m_isCollision = true;
		//printf("土台とアームの衝突\n");
		//m_EndSim = true;
		return;
	}

	// アーム同士の衝突
	flag = false;
	for (int i=0;i<ARM_JOINT_NUM;i++)
	{
		for ( int j=0;j<ARM_JOINT_NUM;j++ )
		{
			if      ( b1 == m_ArmLeft[i].bodyID && b2 == m_ArmRight[j].bodyID ) flag = true;
			else if ( b2 == m_ArmLeft[i].bodyID && b1 == m_ArmRight[j].bodyID ) flag = true;
		}
	}

	if ( n>0 && flag == true )
	{
		m_isCollision = true;
		//printf("アーム同士の衝突\n");
		//m_EndSim = true;
		return;
	}

	// 障害物と衝突
	flag = false;
	for (int i=0;i<(int)m_Obstacle.size();i++)
	{
		if ( b1 == m_Obstacle[i].bodyID || b2 == m_Obstacle[i].bodyID ) flag = true;
	}

	if ( n>0 && flag == true )
	{
		m_isCollision = true;
		//printf("障害物と衝突\n");
		//m_EndSim = true;
		return;
	}

	//int isGround = ( (o1==m_ground)||(o2==m_ground) );
	return;

	for ( int i=0;i<n;i++ )
	{
		contact[i].surface.mode = dContactBounce;//|dContactSoftCFM|dContactSoftERP;
		contact[i].surface.bounce = 0.0;
		//contact[i].surface.soft_cfm = 0.0;
		//contact[i].surface.soft_erp = 1.0;

		dJointID c = dJointCreateContact(m_WorldID, m_contactgroup, &contact[i]);
		dJointAttach(c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
	}
}

void ArmSim::MakeArm()
{
	dMass mass; // 質量パラメータ

	// 顔の生成
	m_Face[0].bodyID = dBodyCreate(m_WorldID);
	dMassSetZero(&mass);
	m_Face[0].x = 0.1;
	m_Face[0].y = 0.2;
	m_Face[0].z = 0.1;
	dMassSetBoxTotal(&mass, 1, m_Face[0].x, m_Face[0].y, m_Face[0].z);
	dBodySetMass(m_Face[0].bodyID, &mass);

	dBodySetPosition(m_Face[0].bodyID, 0.1, 0, 1.5); // 位置セット

	m_Face[1].bodyID = dBodyCreate(m_WorldID);
	dMassSetZero(&mass);
	m_Face[1].x = 0.09;
	m_Face[1].y = 0.05;
	m_Face[1].z = 0.16;
	dMassSetBoxTotal(&mass, 1, m_Face[1].x, m_Face[1].y, m_Face[1].z);
	dBodySetMass(m_Face[1].bodyID, &mass);

	dBodySetPosition(m_Face[1].bodyID, 0.1, 0, 1.39); // 位置セット

	m_Face[2].bodyID = dBodyCreate(m_WorldID);
	dMassSetZero(&mass);
	m_Face[2].r = 0.03;
	dMassSetSphereTotal(&mass, 1, m_Face[2].r);
	dBodySetMass(m_Face[2].bodyID, &mass);

	dBodySetPosition(m_Face[2].bodyID, 0.16, 0.07, 1.5); // 位置セット

	m_Face[3].bodyID = dBodyCreate(m_WorldID);
	dMassSetZero(&mass);
	m_Face[3].r = 0.03;
	dMassSetSphereTotal(&mass, 1, m_Face[3].r);
	dBodySetMass(m_Face[3].bodyID, &mass);

	dBodySetPosition(m_Face[3].bodyID, 0.16, -0.07, 1.5); // 位置セット


	dJointID FaceFixed[4];

	for ( int i=0;i<4;i++ ) 
	{
		FaceFixed[i] = dJointCreateFixed(m_WorldID, 0);	
		dJointAttach(FaceFixed[i], m_Face[i].bodyID, 0);
		dJointSetFixed(FaceFixed[i]);
	}


	// 土台の生成
	m_base.bodyID = dBodyCreate(m_WorldID);
	dMassSetZero(&mass);
	dMassSetBoxTotal(&mass, m_base.m, m_base.r, m_base.r, m_base.l);
	dBodySetMass(m_base.bodyID, &mass);

	m_base.geomID = dCreateBox(m_SpaceID, m_base.r, 2*m_base.r, m_base.l);
	dGeomSetBody(m_base.geomID, m_base.bodyID);

	dBodySetLinearVel(m_base.bodyID,0,0,0);
	dBodySetAngularVel(m_base.bodyID,0,0,0);

	dBodySetPosition(m_base.bodyID, 0, 0, m_base.l/2); // 位置セット


	// アームの生成
	dReal ArmR[ARM_JOINT_NUM];
	dReal ArmL[ARM_JOINT_NUM];
	dReal ArmM[ARM_JOINT_NUM];
	ArmR[0] = ARM0_R;  ArmL[0] = ARM0_L;  ArmM[0] = ARM0_M;
	ArmR[1] = ARM1_R;  ArmL[1] = ARM1_L;  ArmM[1] = ARM1_M;
	ArmR[2] = ARM2_R;  ArmL[2] = ARM2_L;  ArmM[2] = ARM2_M;
	ArmR[3] = ARM3_R;  ArmL[3] = ARM3_L;  ArmM[3] = ARM3_M;
	ArmR[4] = ARM4_R;  ArmL[4] = ARM4_L;  ArmM[4] = ARM4_M;
	ArmR[5] = ARM5_R;  ArmL[5] = ARM5_L;  ArmM[5] = ARM5_M;
	ArmR[6] = ARM6_R;  ArmL[6] = ARM6_L;  ArmM[6] = ARM6_M;
	ArmR[7] = ARM6_R;  ArmL[7] = ARM6_L;  ArmM[7] = ARM6_M;

	// アームの根元の座標
	dReal ArmLeftOrigin[3]  ={0.102,  0.0741, 0.8};
	dReal ArmRightOrigin[3] ={0.102, -0.0741, 0.8};

	dReal ArmLeftPosX[ARM_JOINT_NUM]  = {0};
	dReal ArmLeftPosY[ARM_JOINT_NUM]  = {0};
	dReal ArmLeftPosZ[ARM_JOINT_NUM]  = {0};

	dReal ArmRightPosX[ARM_JOINT_NUM] = {0};
	dReal ArmRightPosY[ARM_JOINT_NUM] = {0};
	dReal ArmRightPosZ[ARM_JOINT_NUM] = {0};

	dReal Anc_LX[ARM_JOINT_NUM] = {0};  dReal Anc_RX[ARM_JOINT_NUM] = {0};
	dReal Anc_LY[ARM_JOINT_NUM] = {0};  dReal Anc_RY[ARM_JOINT_NUM] = {0};
	dReal Anc_LZ[ARM_JOINT_NUM] = {0};  dReal Anc_RZ[ARM_JOINT_NUM] = {0};

	m_ArmLeft[0].Rotation = 0;
	m_ArmLeft[1].Rotation = 90 * M_PI/180;
	m_ArmLeft[2].Rotation = 90 * M_PI/180;
	m_ArmLeft[3].Rotation = 0;
	m_ArmLeft[4].Rotation = 0;
	m_ArmLeft[5].Rotation = 90 * M_PI/180;

	m_ArmRight[0].Rotation = 0;
	m_ArmRight[1].Rotation = 90 * M_PI/180;
	m_ArmRight[2].Rotation = 90 * M_PI/180;
	m_ArmRight[3].Rotation = 0;
	m_ArmRight[4].Rotation = 0;
	m_ArmRight[5].Rotation = 90 * M_PI/180;

	// アームの第一関節の一番を下を基準とした時の各関節の位置を運動学によって求める
	GetArmLeftPos(ArmLeftPosX, ArmLeftPosY, ArmLeftPosZ,
		          Anc_LX,      Anc_LY,      Anc_LZ);
	GetArmRightPos(ArmRightPosX,ArmRightPosY,ArmRightPosZ,
		           Anc_RX,      Anc_RY,      Anc_RZ);

	dReal Ax_X[ARM_JOINT_NUM] = {0,0,0,0,0,0,0,0};
	dReal Ax_Y[ARM_JOINT_NUM] = {0,1,1,0,1,0,1,1};
	dReal Ax_Z[ARM_JOINT_NUM] = {1,0,0,1,0,1,0,0};

	for ( int i=0;i<ARM_JOINT_NUM;i++ )
	{
		// 左
		m_ArmLeft[i].bodyID  = dBodyCreate(m_WorldID);
		dMassSetZero(&mass);
		dMassSetCapsuleTotal(&mass, ArmM[i], 1, ArmR[i], ArmL[i]); // 大きさ指定
		dBodySetMass(m_base.bodyID, &mass);

		m_ArmLeft[i].geomID = dCreateCapsule(m_SpaceID, ArmR[i], ArmL[i]);
		dGeomSetBody(m_ArmLeft[i].geomID, m_ArmLeft[i].bodyID);

		//dRFromAxisAndAngle(R, Ax_X[i], Ax_Y[i], Ax_Z[i], m_ArmLeft[i].Rotation);  // 初期角度指定

		dBodySetLinearVel(m_ArmLeft[i].bodyID,0,0,0);
		dBodySetAngularVel(m_ArmLeft[i].bodyID,0,0,0);

		dBodySetRotation(m_ArmLeft[i].bodyID, m_ArmLeft[i].R); // 回転セット
		dBodySetPosition(m_ArmLeft[i].bodyID, ArmLeftOrigin[0] + ArmLeftPosX[i], 
			                                  ArmLeftOrigin[1] + ArmLeftPosY[i], 
											  ArmLeftOrigin[2] + ArmLeftPosZ[i]); // 位置指定

		// 右
		m_ArmRight[i].bodyID = dBodyCreate(m_WorldID);
		dMassSetZero(&mass);
		dMassSetCapsuleTotal(&mass, ArmM[i], 1, ArmR[i], ArmL[i]); // 大きさ指定
		dBodySetMass(m_base.bodyID, &mass);

		m_ArmRight[i].geomID = dCreateCapsule(m_SpaceID, ArmR[i], ArmL[i]);
		dGeomSetBody(m_ArmRight[i].geomID, m_ArmRight[i].bodyID);

		//dRFromAxisAndAngle(R, Ax_X[i], Ax_Y[i], Ax_Z[i], m_ArmRight[i].Rotation);  // 初期角度指定

		dBodySetLinearVel(m_ArmRight[i].bodyID,0,0,0);
		dBodySetAngularVel(m_ArmRight[i].bodyID,0,0,0);

		dBodySetRotation(m_ArmRight[i].bodyID, m_ArmRight[i].R); // 回転セット
		dBodySetPosition(m_ArmRight[i].bodyID, ArmRightOrigin[0] + ArmRightPosX[i], 
			                                   ArmRightOrigin[1] + ArmRightPosY[i], 
											   ArmRightOrigin[2] + ArmRightPosZ[i]); // 位置指定		
	}

	// 掴む物体の生成
	if ( m_GraspHand == GRASP_LEFT )
	{
		m_Object.bodyID = dBodyCreate(m_WorldID);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass, m_Object.m, m_Object.x, m_Object.y, m_Object.z);
		dBodySetMass(m_Object.bodyID, &mass);

		m_Object.geomID = dCreateBox(m_SpaceID, m_Object.x, m_Object.y, m_Object.z);
		dGeomSetBody(m_Object.geomID, m_Object.bodyID);

		dBodySetPosition(m_Object.bodyID, ArmLeftOrigin[0] + ArmLeftPosX[6],
			                              ArmLeftOrigin[1] + ArmLeftPosY[6] - 0.02 - 2*ARM6_R, 
										  ArmLeftOrigin[2] + ArmLeftPosZ[6] ); // 位置セット
	}
	else if ( m_GraspHand == GRASP_RIGHT )
	{
		m_Object.bodyID = dBodyCreate(m_WorldID);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass, m_Object.m, m_Object.x, m_Object.y, m_Object.z);
		dBodySetMass(m_Object.bodyID, &mass);

		m_Object.geomID = dCreateBox(m_SpaceID, m_Object.x, m_Object.y, m_Object.z);
		dGeomSetBody(m_Object.geomID, m_Object.bodyID);

		dBodySetPosition(m_Object.bodyID, ArmRightOrigin[0] + ArmRightPosX[6],
			                              ArmRightOrigin[1] + ArmRightPosY[6] - 0.02 - 2*ARM6_R, 
										  ArmRightOrigin[2] + ArmRightPosZ[6] ); // 位置セット
	}

	// ジョイント生成
	m_Base2Ground = dJointCreateFixed(m_WorldID, 0);
	for ( int i=0;i<ARM_JOINT_NUM;i++ ) 
	{
		m_ArmLeftJoint[i]  = dJointCreateHinge(m_WorldID, 0);
		m_ArmRightJoint[i] = dJointCreateHinge(m_WorldID, 0);
	}
	if ( m_GraspHand != GRASP_NOTHING ) m_Object2Arm = dJointCreateFixed(m_WorldID, 0);

	// ジョイントでパーツを結合
	dJointAttach(m_Base2Ground, m_base.bodyID, 0);
	dJointSetFixed(m_Base2Ground);

	// 掴んだ物体
	if ( m_GraspHand == GRASP_LEFT )
	{
		dJointAttach(m_Object2Arm, m_Object.bodyID, m_ArmLeft[5].bodyID);
		dJointSetFixed(m_Object2Arm);
	}
	else if ( m_GraspHand == GRASP_RIGHT )
	{
		dJointAttach(m_Object2Arm, m_Object.bodyID, m_ArmRight[5].bodyID);
		dJointSetFixed(m_Object2Arm);
	}

	// 左
	dJointAttach(m_ArmLeftJoint[0], m_base.bodyID, m_ArmLeft[0].bodyID);
	dJointSetHingeParam(m_ArmLeftJoint[0], dParamLoStop, -M_PI);
	dJointSetHingeParam(m_ArmLeftJoint[0], dParamHiStop,  M_PI);

	for ( int i=1;i<ARM_JOINT_NUM-1;i++ ) 
	{
		dJointAttach(m_ArmLeftJoint[i], m_ArmLeft[i-1].bodyID, m_ArmLeft[i].bodyID);
		dJointSetHingeParam(m_ArmLeftJoint[i], dParamLoStop, -M_PI);
		dJointSetHingeParam(m_ArmLeftJoint[i], dParamHiStop,  M_PI);
	}
	dJointAttach(m_ArmLeftJoint[ARM_JOINT_NUM-1], m_ArmLeft[ARM_JOINT_NUM-3].bodyID, 
             									  m_ArmLeft[ARM_JOINT_NUM-1].bodyID); // 指
	dJointSetHingeParam(m_ArmLeftJoint[ARM_JOINT_NUM-1], dParamLoStop, -M_PI);
	dJointSetHingeParam(m_ArmLeftJoint[ARM_JOINT_NUM-1], dParamHiStop,  M_PI);

	// 右
	dJointAttach(m_ArmRightJoint[0], m_base.bodyID, m_ArmRight[0].bodyID);
	dJointSetHingeParam(m_ArmRightJoint[0], dParamLoStop, -M_PI);
	dJointSetHingeParam(m_ArmRightJoint[0], dParamHiStop,  M_PI);

	for ( int i=1;i<ARM_JOINT_NUM-1;i++ )
	{
		dJointAttach(m_ArmRightJoint[i], m_ArmRight[i-1].bodyID, m_ArmRight[i].bodyID);
		dJointSetHingeParam(m_ArmRightJoint[i], dParamLoStop, -M_PI);
		dJointSetHingeParam(m_ArmRightJoint[i], dParamHiStop,  M_PI);
	}
	dJointAttach(m_ArmRightJoint[ARM_JOINT_NUM-1], m_ArmRight[ARM_JOINT_NUM-3].bodyID, 
                                                   m_ArmRight[ARM_JOINT_NUM-1].bodyID); // 指
	dJointSetHingeParam(m_ArmRightJoint[ARM_JOINT_NUM-1], dParamLoStop, -M_PI);
	dJointSetHingeParam(m_ArmRightJoint[ARM_JOINT_NUM-1], dParamHiStop,  M_PI);

	// 関節中心点
	for ( int i=0;i<ARM_JOINT_NUM;i++ ) 
	{
		dJointSetHingeAnchor(m_ArmLeftJoint[i], ArmLeftOrigin[0] + Anc_LX[i],
			                                    ArmLeftOrigin[1] + Anc_LY[i],
												ArmLeftOrigin[2] + Anc_LZ[i]);
	}

	for ( int i=0;i<ARM_JOINT_NUM;i++ ) 
	{
		dJointSetHingeAnchor(m_ArmRightJoint[i], ArmRightOrigin[0] + Anc_RX[i], 
			                                     ArmRightOrigin[1] + Anc_RY[i], 
												 ArmRightOrigin[2] + Anc_RZ[i]);
	}

	// 回転軸
	for ( int i=0;i<ARM_JOINT_NUM;i++ ) dJointSetHingeAxis(m_ArmLeftJoint[i],  Ax_X[i], Ax_Y[i], Ax_Z[i]);
	for ( int i=0;i<ARM_JOINT_NUM;i++ ) dJointSetHingeAxis(m_ArmRightJoint[i], Ax_X[i], Ax_Y[i], Ax_Z[i]);
}

void ArmSim::ArmControl()
{
	bool Inited = true;
	bool done;
	bool Balanced = true;

	if ( m_Inited == false ) done = false;
	else                     done = true;

	dReal AdjustParam[ARM_JOINT_NUM] = {-1.0,
	                                     1.0,
	                                     1.0,
	                                     1.0,
	                                    -1.0,
	                                     1.0,
	                                     1.0,
	                                     1.0};

	for ( int i=0;i<ARM_JOINT_NUM;i++ )
	{
		dReal t;
		dReal s;

		// 左
		t = dJointGetHingeAngle(m_ArmLeftJoint[i]); // 現在の角度を取得
		s = AdjustScope(m_InitAngleL[i]);

		m_JointAngleL[i] = t * AdjustParam[i]; // GUIで使う

		dReal p1 = AdjustScope(t-s);
		dReal p2 = AdjustScope( t - AdjustScope( m_TargetAngleL[i])*AdjustParam[i] );


		// 右
		t = dJointGetHingeAngle(m_ArmRightJoint[i]); // 現在の角度を取得
		s = AdjustScope(m_InitAngleR[i]);

		m_JointAngleR[i] = t * AdjustParam[i]; // GUIで使う

		dReal q1 = AdjustScope(t-s);
		dReal q2 = AdjustScope( t - AdjustScope( m_TargetAngleR[i])*AdjustParam[i] );

		if ( m_Inited == false )
		{
			if ( p1 > 1.0*M_PI/180 )
			{
				m_VelosityL[i] = -3.0;
				Inited = false;
			}
			else if ( p1 < -1.0*M_PI/180 )
			{
				m_VelosityL[i] = 3.0;
				Inited = false;
			}
			else 
			{
				m_VelosityL[i] = 0;
			}

			if ( q1 > 1.0*M_PI/180 )
			{
				m_VelosityR[i] = -3.0;
				Inited = false;
			}
			else if ( q1 < -1.0*M_PI/180 )
			{
				m_VelosityR[i] = 3.0;
				Inited = false;
			}
			else 
			{
				m_VelosityR[i] = 0;
			}
		}
		else if ( m_UseManual )
		{
			// 手動
		}
		else
		{
			if ( p2 > 1.0*M_PI/180 )
			{
				m_VelosityL[i] = -0.4;
				Balanced = false;
			}
			else if ( p2 < -1.0*M_PI/180 )
			{
				m_VelosityL[i] = 0.4;
				Balanced = false;
			}
			else 
			{
				m_VelosityL[i] = 0;
			}

			if ( q2 > 1.0*M_PI/180 )
			{
				m_VelosityR[i] = -0.4;
				Balanced = false;
			}
			else if ( q2 < -1.0*M_PI/180 )
			{
				m_VelosityR[i] = 0.4;
				Balanced = false;
			}
			else 
			{
				m_VelosityR[i] = 0;
			}
		}

		dJointSetHingeParam(m_ArmLeftJoint[i], dParamVel, m_VelosityL[i]);
		dJointSetHingeParam(m_ArmLeftJoint[i], dParamFMax, 5000);

		dJointSetHingeParam(m_ArmRightJoint[i], dParamVel, m_VelosityR[i]);
		dJointSetHingeParam(m_ArmRightJoint[i], dParamFMax, 5000);
	}

	if ( Inited == true ) 
	{
		if ( done == false ) MakeObstacle(); // 関節角の初期化が終了した時点で障害物生成
		m_Inited = true;
	}

	if ( Balanced == true && m_Inited == true )
	{
		m_isBalanced = true;
	}
}

void ArmSim::DrawArm()
{
	// 顔の描画
	dsSetColor(0,0,1);
	{
		const dReal *pos = dBodyGetPosition(m_Face[0].bodyID);
		const dReal *R   = dBodyGetRotation(m_Face[0].bodyID);
		const double s[3] = {m_Face[0].x, m_Face[0].y, m_Face[0].z};
		dsDrawBoxD(pos, R, s);
	}
	dsSetColor(1,1,1);
	{
		const dReal *pos = dBodyGetPosition(m_Face[1].bodyID);
		const dReal *R   = dBodyGetRotation(m_Face[1].bodyID);
		const double s[3] = {m_Face[1].x, m_Face[1].y, m_Face[1].z};
		dsDrawBoxD(pos, R, s);
	}
	dsSetColor(1,1,1);
	{
		const dReal *pos = dBodyGetPosition(m_Face[2].bodyID);
		const dReal *R   = dBodyGetRotation(m_Face[2].bodyID);
		dsDrawSphereD(pos, R, m_Face[2].r);
	}
	{
		const dReal *pos = dBodyGetPosition(m_Face[3].bodyID);
		const dReal *R   = dBodyGetRotation(m_Face[3].bodyID);
		dsDrawSphereD(pos, R, m_Face[3].r);
	}

	// 土台の描画
	{
		const dReal *pos = dBodyGetPosition(m_base.bodyID);
		const dReal *R   = dBodyGetRotation(m_base.bodyID);
		const double s[3] = {m_base.r, 2*m_base.r, m_base.l};
		dsDrawBoxD(pos, R, s);
	}

	// オブジェクトの描画
	if ( m_GraspHand )
	{
		dsSetColor(0.2,1,0);
		{
			const dReal *pos = dBodyGetPosition(m_Object.bodyID);
			const dReal *R   = dBodyGetRotation(m_Object.bodyID);
			const double s[3] = {m_Object.x, m_Object.y, m_Object.z};
			dsDrawBoxD(pos, R, s);
		}
	}

	// アームの描画
	dReal ArmR[ARM_JOINT_NUM];
	dReal ArmL[ARM_JOINT_NUM];
	ArmR[0] = ARM0_R;  ArmL[0] = ARM0_L;
	ArmR[1] = ARM1_R;  ArmL[1] = ARM1_L;
	ArmR[2] = ARM2_R;  ArmL[2] = ARM2_L;
	ArmR[3] = ARM3_R;  ArmL[3] = ARM3_L;
	ArmR[4] = ARM4_R;  ArmL[4] = ARM4_L;
	ArmR[5] = ARM5_R;  ArmL[5] = ARM5_L;
	ArmR[6] = ARM6_R;  ArmL[6] = ARM6_L;
	ArmR[7] = ARM6_R;  ArmL[7] = ARM6_L;

	for ( int i=0;i<ARM_JOINT_NUM;i++ )
	{
		{
			const dReal *pos = dBodyGetPosition(m_ArmLeft[i].bodyID);
			const dReal *R   = dBodyGetRotation(m_ArmLeft[i].bodyID);
			dsSetColor(1,1,1);
			dsDrawCapsuleD(pos, R, ArmL[i], ArmR[i]);
		}

		{
			const dReal *pos = dBodyGetPosition(m_ArmRight[i].bodyID);
			const dReal *R   = dBodyGetRotation(m_ArmRight[i].bodyID);
			dsDrawCapsuleD(pos, R, ArmL[i], ArmR[i]);
		}
	}
}

// 運動学
void ArmSim::GetArmLeftPos(dReal Px[ARM_JOINT_NUM], dReal Py[ARM_JOINT_NUM], dReal Pz[ARM_JOINT_NUM],
		                   dReal Ax[ARM_JOINT_NUM], dReal Ay[ARM_JOINT_NUM], dReal Az[ARM_JOINT_NUM])
{
	// 無駄に回転行列で姿勢を設定しているが、特にこのやり方である必要はない
	// 作った後でわかったことなのでこのままにしている
	dReal Vector[3] = {0.0};
	dReal R0[3][3]  = {0.0};
	dReal R1[3][3]  = {0.0};

	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationY(R1,R0,m_ArmLeft[1].Rotation + m_ArmLeft[2].Rotation);

	// 第0関節
	Ax[0] = 0;
	Ay[0] = 0;
	Az[0] = 0;

	// 第0アーム重心
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	GetdMatrix3(m_ArmLeft[0].R,R1);
	Px[0] = 0; 
	Py[0] = 0; 
	Pz[0] = ARM0_L/2;


	// 第1関節
	Vector[0] = 0;
	Vector[1] = 0.18; 
	Vector[2] = ARM0_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	GetdMatrix3(m_ArmLeft[0].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[1] = Ax[0] + Vector[0];
	Ay[1] = Ay[0] + Vector[1]; 
	Az[1] = Az[0] + Vector[2];

	// 第1アーム重心
	Vector[0] = 0; // 関節中心点からの相対位置
	Vector[1] = 0;
	Vector[2] = ARM1_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	GetdMatrix3(m_ArmLeft[1].R,R0);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Px[1] = Ax[1] + Vector[0]; // 絶対座標 
	Py[1] = Ay[1] + Vector[1]; 
	Pz[1] = Az[1] + Vector[2];


	// 第2関節
	Vector[0] = 0; // 初期値
	Vector[1] = -0.06; 
	Vector[2] = ARM1_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[2] = Ax[1] + Vector[0]; // 関節中心点
	Ay[2] = Ay[1] + Vector[1]; 
	Az[2] = Az[1] + Vector[2];

	// 第2アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM2_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	GetdMatrix3(m_ArmLeft[2].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[2] = Ax[2] + Vector[0];
	Py[2] = Ay[2] + Vector[1]; 
	Pz[2] = Az[2] + Vector[2];


	// 第3関節
	Vector[0] = 0; // 初期値
	Vector[1] = 0; 
	Vector[2] = ARM2_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[3] = Ax[2] + Vector[0]; // 関節中心点
	Ay[3] = Ay[2] + Vector[1]; 
	Az[3] = Az[2] + Vector[2];

	// 第3アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM3_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	GetdMatrix3(m_ArmLeft[3].R,R0);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Px[3] = Ax[3] + Vector[0];
	Py[3] = Ay[3] + Vector[1]; 
	Pz[3] = Az[3] + Vector[2];


	// 第4関節
	Vector[0] = 0; // 初期値
	Vector[1] = 0; 
	Vector[2] = ARM3_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[4] = Ax[3] + Vector[0]; // 関節中心点
	Ay[4] = Ay[3] + Vector[1]; 
	Az[4] = Az[3] + Vector[2];

	// 第4アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM4_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	GetdMatrix3(m_ArmLeft[4].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[4] = Ax[4] + Vector[0];
	Py[4] = Ay[4] + Vector[1]; 
	Pz[4] = Az[4] + Vector[2];


	// 第5関節
	Vector[0] = 0; // 初期値
	Vector[1] = 0; 
	Vector[2] = ARM4_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[5] = Ax[4] + Vector[0]; // 関節中心点
	Ay[5] = Ay[4] + Vector[1]; 
	Az[5] = Az[4] + Vector[2];

	// 第5アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM5_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[5].Rotation);
	GetdMatrix3(m_ArmLeft[5].R,R0);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Px[5] = Ax[5] + Vector[0];
	Py[5] = Ay[5] + Vector[1]; 
	Pz[5] = Az[5] + Vector[2];


	// 指
	Vector[0] = 0; // 初期値
	Vector[1] = 0.01; 
	Vector[2] = ARM5_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[5].Rotation);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[6] = Ax[5] + Vector[0]; // 関節中心点
	Ay[6] = Ay[5] + Vector[1]; 
	Az[6] = Az[5] + Vector[2];

	// 指重心
	Vector[0] = 0;
	Vector[1] = 0.01;
	Vector[2] = ARM6_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[5].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[6].Rotation);
	GetdMatrix3(m_ArmLeft[6].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[6] = Ax[6] + Vector[0];
	Py[6] = Ay[6] + Vector[1]; 
	Pz[6] = Az[6] + Vector[2];


	// 指
	Vector[0] = 0; // 初期値
	Vector[1] = -0.01; 
	Vector[2] = ARM5_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[5].Rotation);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[7] = Ax[5] + Vector[0]; // 関節中心点
	Ay[7] = Ay[5] + Vector[1]; 
	Az[7] = Az[5] + Vector[2];

	// 指重心
	Vector[0] = 0;
	Vector[1] = -0.01;
	Vector[2] = ARM6_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmLeft[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmLeft[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmLeft[5].Rotation);
	Get3DRotationY(R1,R0,m_ArmLeft[7].Rotation);
	GetdMatrix3(m_ArmLeft[7].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[7] = Ax[7] + Vector[0];
	Py[7] = Ay[7] + Vector[1]; 
	Pz[7] = Az[7] + Vector[2];
}

void ArmSim::GetArmRightPos(dReal Px[ARM_JOINT_NUM], dReal Py[ARM_JOINT_NUM], dReal Pz[ARM_JOINT_NUM],
		                    dReal Ax[ARM_JOINT_NUM], dReal Ay[ARM_JOINT_NUM], dReal Az[ARM_JOINT_NUM])
{
	// 基本的に↑と同じです。回転角を左アーム用を使うか右アーム用を使うかのみ違います。
	dReal Vector[3] = {0.0};
	dReal R0[3][3]  = {0.0};
	dReal R1[3][3]  = {0.0};

	// 第0関節
	Ax[0] = 0;
	Ay[0] = 0;
	Az[0] = 0;

	// 第0アーム重心
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	GetdMatrix3(m_ArmRight[0].R,R1);
	Px[0] = 0; 
	Py[0] = 0; 
	Pz[0] = ARM0_L/2;


	// 第1関節
	Vector[0] = 0;
	Vector[1] = 0.18; 
	Vector[2] = ARM0_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	GetdMatrix3(m_ArmRight[0].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[1] = Ax[0] + Vector[0];
	Ay[1] = Ay[0] + Vector[1]; 
	Az[1] = Az[0] + Vector[2];

	// 第1アーム重心
	Vector[0] = 0; // 関節中心点からの相対位置
	Vector[1] = 0;
	Vector[2] = ARM1_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	GetdMatrix3(m_ArmRight[1].R,R0);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Px[1] = Ax[1] + Vector[0]; // 絶対座標 
	Py[1] = Ay[1] + Vector[1]; 
	Pz[1] = Az[1] + Vector[2];


	// 第2関節
	Vector[0] = 0; // 初期値
	Vector[1] = -0.06; 
	Vector[2] = ARM1_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[2] = Ax[1] + Vector[0]; // 関節中心点
	Ay[2] = Ay[1] + Vector[1]; 
	Az[2] = Az[1] + Vector[2];

	// 第2アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM2_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	GetdMatrix3(m_ArmRight[2].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[2] = Ax[2] + Vector[0];
	Py[2] = Ay[2] + Vector[1]; 
	Pz[2] = Az[2] + Vector[2];


	// 第3関節
	Vector[0] = 0; // 初期値
	Vector[1] = 0; 
	Vector[2] = ARM2_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[3] = Ax[2] + Vector[0]; // 関節中心点
	Ay[3] = Ay[2] + Vector[1]; 
	Az[3] = Az[2] + Vector[2];

	// 第3アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM3_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	GetdMatrix3(m_ArmRight[3].R,R0);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Px[3] = Ax[3] + Vector[0];
	Py[3] = Ay[3] + Vector[1]; 
	Pz[3] = Az[3] + Vector[2];


	// 第4関節
	Vector[0] = 0; // 初期値
	Vector[1] = 0; 
	Vector[2] = ARM3_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[4] = Ax[3] + Vector[0]; // 関節中心点
	Ay[4] = Ay[3] + Vector[1]; 
	Az[4] = Az[3] + Vector[2];

	// 第4アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM4_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	GetdMatrix3(m_ArmRight[4].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[4] = Ax[4] + Vector[0];
	Py[4] = Ay[4] + Vector[1]; 
	Pz[4] = Az[4] + Vector[2];


	// 第5関節
	Vector[0] = 0; // 初期値
	Vector[1] = 0; 
	Vector[2] = ARM4_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[5] = Ax[4] + Vector[0]; // 関節中心点
	Ay[5] = Ay[4] + Vector[1]; 
	Az[5] = Az[4] + Vector[2];

	// 第5アーム重心
	Vector[0] = 0;
	Vector[1] = 0;
	Vector[2] = ARM5_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[5].Rotation);
	GetdMatrix3(m_ArmRight[5].R,R0);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Px[5] = Ax[5] + Vector[0];
	Py[5] = Ay[5] + Vector[1]; 
	Pz[5] = Az[5] + Vector[2];


	// 指
	Vector[0] = 0; // 初期値
	Vector[1] = 0.01; 
	Vector[2] = ARM5_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[5].Rotation);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[6] = Ax[5] + Vector[0]; // 関節中心点
	Ay[6] = Ay[5] + Vector[1]; 
	Az[6] = Az[5] + Vector[2];

	// 指重心
	Vector[0] = 0;
	Vector[1] = 0.01;
	Vector[2] = ARM6_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[5].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[6].Rotation);
	GetdMatrix3(m_ArmRight[6].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[6] = Ax[6] + Vector[0];
	Py[6] = Ay[6] + Vector[1]; 
	Pz[6] = Az[6] + Vector[2];


	// 指
	Vector[0] = 0; // 初期値
	Vector[1] = -0.01; 
	Vector[2] = ARM5_L;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[5].Rotation);
	GetMatrix33x13(Vector, R0); // 最終的な回転行列と相対ベクトルの掛け算
	Ax[7] = Ax[5] + Vector[0]; // 関節中心点
	Ay[7] = Ay[5] + Vector[1]; 
	Az[7] = Az[5] + Vector[2];

	// 指重心
	Vector[0] = 0;
	Vector[1] = -0.01;
	Vector[2] = ARM6_L/2;
	Get3DUnitMatrix(R0); // 単位行列セット
	Get3DRotationZ(R1,R0,m_ArmRight[0].Rotation);
	Get3DRotationY(R0,R1,m_ArmRight[1].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[2].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[3].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[4].Rotation);
	Get3DRotationZ(R0,R1,m_ArmRight[5].Rotation);
	Get3DRotationY(R1,R0,m_ArmRight[7].Rotation);
	GetdMatrix3(m_ArmRight[7].R,R1);
	GetMatrix33x13(Vector, R1); // 最終的な回転行列と相対ベクトルの掛け算
	Px[7] = Ax[7] + Vector[0];
	Py[7] = Ay[7] + Vector[1]; 
	Pz[7] = Az[7] + Vector[2];
}

// 回転行列 3x3行列に各軸を中心に回転させたときの回転行列をかけた結果を返します
void ArmSim::Get3DRotationX(dReal Result[3][3], const dReal PosSource[3][3], dReal Radian)
{
	//未実装
}

void ArmSim::Get3DRotationY(dReal Result[3][3], const dReal PosSource[3][3], dReal Radian)
{
	Result[0][0] = PosSource[0][0] * cos(Radian) - PosSource[0][2] * sin(Radian);
	Result[0][1] = PosSource[0][1];
	Result[0][2] = PosSource[0][0] * sin(Radian) + PosSource[0][2] * cos(Radian);

	Result[1][0] = PosSource[1][0] * cos(Radian) - PosSource[1][2] * sin(Radian);
	Result[1][1] = PosSource[1][1];
	Result[1][2] = PosSource[1][0] * sin(Radian) + PosSource[1][2] * cos(Radian);

	Result[2][0] = PosSource[2][0] * cos(Radian) - PosSource[2][2] * sin(Radian);
	Result[2][1] = PosSource[2][1];
	Result[2][2] = PosSource[2][0] * sin(Radian) + PosSource[2][2] * cos(Radian);
}

void ArmSim::Get3DRotationZ(dReal Result[3][3], const dReal PosSource[3][3], dReal Radian)
{
	Result[0][0] = PosSource[0][0] * cos(Radian) + PosSource[0][1] * sin(Radian);
	Result[0][1] = PosSource[0][1] * cos(Radian) - PosSource[0][0] * sin(Radian);
	Result[0][2] = PosSource[0][2];

	Result[1][0] = PosSource[1][0] * cos(Radian) + PosSource[1][1] * sin(Radian);
	Result[1][1] = PosSource[1][1] * cos(Radian) - PosSource[1][0] * sin(Radian);
	Result[1][2] = PosSource[1][2];

	Result[2][0] = PosSource[2][0] * cos(Radian) + PosSource[2][1] * sin(Radian);
	Result[2][1] = PosSource[2][1] * cos(Radian) - PosSource[2][0] * sin(Radian);
	Result[2][2] = PosSource[2][2];
}

// 単位行列を求める
void ArmSim::Get3DUnitMatrix(dReal Result[3][3])
{
	Result[0][0] = 1.0;
	Result[0][1] = 0.0;
	Result[0][2] = 0.0;

	Result[1][0] = 0.0;
	Result[1][1] = 1.0;
	Result[1][2] = 0.0;

	Result[2][0] = 0.0;
	Result[2][1] = 0.0;
	Result[2][2] = 1.0;
}

// 3x3行列と相対ベクトルの掛け算
void ArmSim::GetMatrix33x13(dReal Result[3], const dReal Source[3][3])
{
	dReal tmp[3];
	tmp[0] = Result[0] * Source[0][0] + Result[1] * Source[0][1] + Result[2] * Source[0][2];
	tmp[1] = Result[0] * Source[1][0] + Result[1] * Source[1][1] + Result[2] * Source[1][2];
	tmp[2] = Result[0] * Source[2][0] + Result[1] * Source[2][1] + Result[2] * Source[2][2];

	for ( int i=0;i<3;i++ ) Result[i] = tmp[i];
}

// 自作回転行列をODE用の回転行列dMatrix3型へ変換
void ArmSim::GetdMatrix3(dMatrix3 Result, const dReal m[3][3])
{
	Result[0] = m[0][0];
	Result[1] = m[0][1];
	Result[2] = m[0][2];
	Result[3] = 0.0;
	Result[4] = m[1][0];
	Result[5] = m[1][1];
	Result[6] = m[1][2];
	Result[7] = 0.0;
	Result[8] = m[2][0];
	Result[9] = m[2][1];
	Result[10] = m[2][2];
	Result[11] = 0.0;
}

// ラジアンを範囲内に収める
dReal ArmSim::AdjustScope(dReal a)
{
	while ( a >  M_PI ) a -= 2*M_PI;
	while ( a < -M_PI ) a += 2*M_PI;
	return a;
}

void ArmSim::MakeObstacle()
{
	dMass mass; // 質量パラメータ

	for ( int i=0;i<(int)m_Obstacle.size();i++ )
	{
		m_Obstacle[i].bodyID = dBodyCreate(m_WorldID);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass, m_Obstacle[i].m, m_Obstacle[i].x, m_Obstacle[i].y, m_Obstacle[i].z);
		dBodySetMass(m_Obstacle[i].bodyID, &mass);

		//m_Obstacle[i].geomID = dCreateBox(m_SpaceID, m_Obstacle[i].x, m_Obstacle[i].y, m_Obstacle[i].z);
		//dGeomSetBody(m_Obstacle[i].geomID, m_Obstacle[i].bodyID);

		dBodySetPosition(m_Obstacle[i].bodyID, m_Obstacle[i].Pos[0], m_Obstacle[i].Pos[1], m_Obstacle[i].Pos[2]); // 位置セット

		m_Obstacle[i].Joint = dJointCreateFixed(m_WorldID, 0);
		// ジョイントでパーツを結合
		dJointAttach(m_Obstacle[i].Joint, m_Obstacle[i].bodyID, 0);
		dJointSetFixed(m_Obstacle[i].Joint);

		//dGeomDisable(m_Obstacle[i].geomID);
	}
}

void ArmSim::DrawObstacle()
{
	//dsSetColor(0.1, 0.7, 0.5);
	dsSetColor(1.0, 0.2, 0.0);
	for ( int i=0;i<(int)m_Obstacle.size();i+=2 )
	{         
		//srand(i*200);
		//double r = (double)rand() / 0xffff;
		//double g = (double)rand() / 0xffff;
		//double b = (double)rand() / 0xffff;

		//dsSetColor(1,g,b);

		const dReal *pos = dBodyGetPosition(m_Obstacle[i].bodyID);
		const dReal *R   = dBodyGetRotation(m_Obstacle[i].bodyID);
		const double s[3] = {m_Obstacle[i].x, m_Obstacle[i].y, m_Obstacle[i].z};
		dsDrawBoxD(pos, R, s);
	}

	//dsSetColor(1.0, 0.2, 0.0);
	for ( int i=1;i<(int)m_Obstacle.size();i+=2 )
	{
		const dReal *pos = dBodyGetPosition(m_Obstacle[i].bodyID);
		const dReal *R   = dBodyGetRotation(m_Obstacle[i].bodyID);
		const double s[3] = {m_Obstacle[i].x, m_Obstacle[i].y, m_Obstacle[i].z};
		dsDrawBoxD(pos, R, s);
	}
}
