/// <summary>首の角度を設定 FIXME: 現状はヨー角のみ</summary>
private static void SetHead(RobotJointAngles robot, Body body)
{
var neck = QuartanionFactory.FromVector4(body.JointOrientations[JointType.Neck].Orientation);
var spine = QuartanionFactory.FromVector4(body.JointOrientations[JointType.SpineShoulder].Orientation);
var neckZ = Quartanion.UnitZ.RotateBy(neck);
//正規直交基底に使う
var spineX = Quartanion.UnitX.RotateBy(spine);
var spineY = Quartanion.UnitY.RotateBy(spine);
var spineZ = Quartanion.UnitZ.RotateBy(spine);
var neckZfromSpine = new Quartanion
{
X = neckZ.Product(spineX),
Y = neckZ.Product(spineY),
Z = neckZ.Product(spineZ)
};
robot.HeadYaw = (float)Math.Asin(neckZfromSpine.X);
}
/// <summary>指定した関節の方向への回転を取得します。</summary>
private static Quartanion GetOrientation(Body body, JointType jtype)
{
return(QuartanionFactory.FromVector4(body.JointOrientations[jtype].Orientation));
}