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)); }