#pragma once

#define ARM_POSE_ELBOW_UP	0
#define ARM_POSE_ELBOW_DOWN	1

#define LEFT_ARM  0
#define RIGHT_ARM 1

bool Pos2JointR( float pos[6] , float joint[6] , int ArmPose );
bool Pos2JointL( float pos[6] , float joint[6] , int ArmPose );
bool CalcInverseKinematics( float pos[6] , float joint[6] , int ArmPose , bool rightArm );

void CalcKinematics( float J[6] , float P[6] );