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");
        }
Exemplo n.º 2
0
        /// <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);
        }
Exemplo n.º 3
0
 /// <summary>指定した関節の方向への回転を取得します。</summary>
 private static Quartanion GetOrientation(Body body, JointType jtype)
 {
     return(QuartanionFactory.FromVector4(body.JointOrientations[jtype].Orientation));
 }