FASTRAKとXNAでオブジェクトを3次元空間上で操作

POLHEMUS FASTRAKを使う
http://www.kosaka-lab.com/tips/2009/12/polhemus-fastrak.php

FASTRAKをC#で使う
http://www.kosaka-lab.com/tips/2009/12/fastrakc.php

FASTRAKとXNAでオブジェクトを3次元空間上で操作
http://www.kosaka-lab.com/tips/2010/11/fastrakxna3.php


FASTRAKのレシーバを動かすことで、3次元空間内のオブジェクトを移動・回転させる方法です。
XNAで実装していますが、DirectXでも同じ理屈です（OpenGLは座標系が違うため一部編集が必要になります）。

クォータニオンを使用します。


このプログラムを改良し、USBカメラなどにレシーバを固定することで、ARを実現させることができます。

プロジェクト作成

VisualC#でXNA GameStudiio->Windowsゲームの「XNAFastrak」というプロジェクトを作成します

コンテンツ準備

このファイルをダウンロード・解凍し、ファイルをソリューションエクスプローラのContentに追加します

プログラム

Game1.csに以下のプログラムを貼り付けます(COM1を自分の環境に合わせて書き換えてください)。

using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;
using System.IO.Ports;

namespace XNAFastrak
{
    public class Game1 : Microsoft.Xna.Framework.Game
    {
        GraphicsDeviceManager graphics;
        SpriteBatch spriteBatch;

        private Model xGen;                 //原点
        private Model xRcv;                //レシーバのXファイル
        private SerialPort serialPort;      //シリアルポート

        //レシーバの座標
        private double TX = 0;
        private double TY = 0;
        private double TZ = 0;

        //くオータニオン
        private double QX = 0;
        private double QY = 0;
        private double QZ = 0;
        private double QW = 0;

        public Game1()
        {
            graphics = new GraphicsDeviceManager(this);
            Content.RootDirectory = "Content";

            //シリアルポート
            this.serialPort = new System.IO.Ports.SerialPort();
            this.serialPort.DataReceived +=
                new System.IO.Ports.SerialDataReceivedEventHandler(this.serialPort_DataReceived);
        }

        protected override void Initialize()
        {
            this.serialPort.PortName = "COM4";//ポート番号（＊自分の環境に合わせて変更すること）
            this.serialPort.BaudRate = 115200;//ビットレート


            try
            {
                //ポートオープン
                this.serialPort.Open();

                this.serialPort.WriteLine("u");             //センチ
                this.serialPort.WriteLine("O1,2,11,1\n");   //書式 マニュアル87ページ
                this.serialPort.WriteLine("b1\n");          //アンボアサイト　マニュアル68ページ
                this.serialPort.WriteLine("H1,0,0,1\n");    //測定半球の設定　マニュアル77ページ
                this.serialPort.WriteLine("C");             //情報を自動送信開始
            }catch(Exception e)
            {
                //ポートオープンに失敗
                Console.WriteLine(e.ToString());
            }
            base.Initialize();
        }

        //ロード
        protected override void LoadContent()
        {
            spriteBatch = new SpriteBatch(GraphicsDevice);

            //レシーバ用Xファイルの読み込み
            this.xRcv = this.Content.Load<Model>("airplane");
            foreach (ModelMesh mesh in this.xRcv.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.View = Matrix.CreateLookAt(new Vector3(0.0f, 50.0f, 50.0f), Vector3.Zero, Vector3.Up);

                    //プロジェクション行列　視野角などの設定
                    effect.Projection = Matrix.CreatePerspectiveFieldOfView(
                      MathHelper.ToRadians(45.0f),
                      (float)this.GraphicsDevice.Viewport.Width / (float)this.GraphicsDevice.Viewport.Height,
                      1.0f,
                      300.0f
                    );
                }
            }


            //原点用Xファイルの読み込み
            this.xGen = this.Content.Load<Model>("cone");
            foreach (ModelMesh mesh in this.xGen.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.View = Matrix.CreateLookAt(new Vector3(0.0f, 50.0f, 50.0f), Vector3.Zero, Vector3.Up);

                    //プロジェクション行列　視野角などの設定
                    effect.Projection = Matrix.CreatePerspectiveFieldOfView(
                      MathHelper.ToRadians(45.0f),
                      (float)this.GraphicsDevice.Viewport.Width / (float)this.GraphicsDevice.Viewport.Height,
                      1.0f,
                      300.0f
                    );
                }
            }  
        }

        //アンロード
        protected override void UnloadContent()
        {
            this.serialPort.Close();    //ポートクローズ
        }

        //更新
        protected override void Update(GameTime gameTime)
        {
            if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
                this.Exit();

            base.Update(gameTime);
        }

        //描画
        protected override void Draw(GameTime gameTime)
        {
            GraphicsDevice.Clear(Color.CornflowerBlue);

            foreach (ModelMesh mesh in this.xGen.Meshes)
            {
                mesh.Draw();
            }


            //レシーバのXファイル
            foreach (ModelMesh mesh in this.xRcv.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.World = Matrix.CreateFromQuaternion(
                        new Quaternion((float)QX, (float)QY, (float)QZ, (float)QW))
                        * Matrix.CreateTranslation((float)TX, (float)TY, (float)TZ);
                }
                mesh.Draw();
            }

            base.Draw(gameTime);
        }

        //データ受信
        private void serialPort_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
        {
            int no = 0;//レシーバ番号

            try
            {
                SerialPort port = (SerialPort)sender;
                string s = port.ReadLine();

                //レシーバー番号を取得
                no = Convert.ToInt32(s.Substring(0, 2)) - 1;

                //位置を取得
                if (no == 0)//レシーバ0番
                {
                    //位置情報
                    TX = -Convert.ToDouble(s.Substring(3, 7));
                    TY = Convert.ToDouble(s.Substring(17, 7));
                    TZ = -Convert.ToDouble(s.Substring(10, 7));

                    //クォータニオン
                    QY = Convert.ToDouble(s.Substring(24, 7));
                    QZ = Convert.ToDouble(s.Substring(31, 7));
                    QX = -Convert.ToDouble(s.Substring(38, 7));
                    QW = Convert.ToDouble(s.Substring(45, 7));
                }
            }
            catch
            {
                Console.WriteLine("データ受信エラー");
            }
        }
    }
}

実行

1.Fastrakの電源を入れて、緑のランプの点滅が終わるのを待ちます。

2.F5キーを押して実行してください。

3.原点(トランスミッタの位置)にコーン、レシーバの位置に飛行機が表示されます。レシーバを動かすと飛行機が移動、回転します。

解説

レシーバの位置・回転情報をXファイルの座標に適応させています。

this.serialPort.WriteLine("O1,2,11,1\n");   //書式 マニュアル87ページ
回転角度をオイラー角ではなくクォータニオンで送信するように設定するコマンドです。

クォータニオンについて解説すると大変な量になるためここでは割愛します。
各自自分で調べてください。