//BoneTagからBoneに対応するベクトルを取得する
public Vector3D getBoneWithTag(BoneTag tag)
{
return(boneDict[tag]);
}
//二つのBoneの角度として設定できるサーボはこいつで設定する
private void bonesToServo(BoneTag bone_a, BoneTag bone_b, ServoTag destServo)
{
servoDict[destServo].SetDestWithDegree(Vector3D.AngleBetween(mBoneDict[bone_a], mBoneDict[bone_b]));
}
//joint二つをもとにBoneTagに対応するBoneをboneDictに登録する
private void jointsToBone(JointType joint_a, JointType joint_b, BoneTag destBone)
{
boneDict[destBone] = MathUtil.JointsToVector3D(mJoints[joint_a], mJoints[joint_b]);
}
