/// <summary>
/// Quaternion で渡された姿勢のうち、X, Y, Z 軸いずれか周り成分を抽出してサーボ角に反映します
/// </summary>
/// <param name="rot">目標姿勢</param>
/// <param name="joint">指定サーボ</param>
/// <returns>回転させた軸成分を除いた残りの回転 Quaternion</returns>
Quaternion ApplyDirectRotation(Quaternion rot, float angle, ModelJoint joint)
{
Quaternion q;
Vector3 axis = Vector3.right;
float direction = (joint.isInverse ? -1f : 1f); // 逆転なら-1
switch (joint.targetAxis)
{
case ModelJoint.Axis.X:
axis = Vector3.right;
break;
case ModelJoint.Axis.Y:
axis = Vector3.up;
break;
case ModelJoint.Axis.Z:
axis = Vector3.forward;
break;
}
var actualAngle = joint.SetServoValue(angle * direction);
//q = Quaternion.AngleAxis(angle, axis);
q = Quaternion.AngleAxis(actualAngle / direction, axis); // 角度制限を考慮
return(Quaternion.Inverse(q) * rot);
}
/// <summary>
/// Quaternion で渡された姿勢のうち、X, Y, Z 軸いずれか周り成分を抽出してサーボ角に反映します
/// </summary>
/// <param name="rot">目標姿勢</param>
/// <param name="joint">指定サーボ</param>
/// <returns>回転させた軸成分を除いた残りの回転 Quaternion</returns>
Quaternion ApplyPartialRotation(Quaternion rot, ModelJoint joint)
{
Quaternion q = rot;
Vector3 axis = Vector3.right;
float direction = (joint.isInverse ? -1f : 1f); // 逆転なら-1
switch (joint.targetAxis)
{
case ModelJoint.Axis.X:
q.y = q.z = 0;
if (q.x < 0)
{
direction = -direction;
}
axis = Vector3.right;
break;
case ModelJoint.Axis.Y:
q.x = q.z = 0;
if (q.y < 0)
{
direction = -direction;
}
axis = Vector3.up;
break;
case ModelJoint.Axis.Z:
q.x = q.y = 0;
if (q.z < 0)
{
direction = -direction;
}
axis = Vector3.forward;
break;
}
if (q.w == 0 && q.x == 0 && q.y == 0 && q.z == 0)
{
Debug.Log("Joint: " + joint.name + " rotation N/A");
q = Quaternion.identity;
}
q.Normalize();
float angle = Mathf.Acos(q.w) * 2.0f * Mathf.Rad2Deg * direction;
var actualAngle = joint.SetServoValue(angle);
return(rot * Quaternion.Inverse(q));
//return rot * Quaternion.Inverse(Quaternion.AngleAxis(actualAngle, axis);
}
