private void SetVectorData(Body body)
{
var neck = QuartanionFactory.FromVector4(body.JointOrientations[JointType.Neck].Orientation);
var spine = QuartanionFactory.FromVector4(body.JointOrientations[JointType.SpineShoulder].Orientation);
var head = QuartanionFactory.FromVector4(body.JointOrientations[JointType.Head].Orientation);
var neckZ = Quartanion.UnitZ.RotateBy(neck);
var spineZ = Quartanion.UnitZ.RotateBy(spine);
this.MyVectorInfo = "neck Z = " + neckZ.ToString("0.00") + ", spine Z = " + spineZ.ToString("0.00");
//正規直交基底に使う
//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)
//};
//this.MyVectorInfo = neckZfromSpine.ToString("0.00");
}
/// <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));
}