１１、kinect




Kinectでのキャプチャ機能を付けました。
OpenNIを使用するのでキャプチャするには下準備としてOpenNIをインストールする必要があります。
OpenNIはバージョンによって動かなかったりする時がたまにあるので
OpenNIで使用するOpenNIのバージョンをzipにしました。
こちらからダウンロードしてください。
なおこのzipに入っているOpenNIのファイルのライセンスについては
http://www.openni.org/
https://github.com/avin2/SensorKinect
をご覧ください。


Kinectでキャプチャするには上のスクリーンショットのように初期姿勢がせんだみつおポーズのモデルを用意します（このポーズは必須）。
(OpenRDBのzipのMedia/KinectSample/hattousinの中にスクリーンショットのデータを同梱しました。)


操作の順番は
kinectメニューの「ライブラリロード」、「kbs読み込み」または「kbs作成」を実行します。
次に初期状態のモーションの長さでは１秒くらいでキャプチャが終了するので
ツールウインドウのプロパティを実行し、モーションの長さを長くします。

モーション長８００くらいが安定しています。
あまり長いと処理が重くなるためか、OpenNIの応答が無くなったりして不安定になります。

次にkinectメニューの「キャプチャ開始」を実行します。
実行したらkinectとの距離を２ｍほど取り、せんだみつおポーズをしてください。

認識されると３Dウインドウ上に自分の赤いシルエットが表示されます。
シルエットが表示されにくい時は、小さく前後に動くとすみやかに認識されます。

３Dモデルが微妙に動いたらキャプチャ開始です。
きれいにキャプチャするにはモーションの演じ手側がキャプチャしにくいポーズを避けるなどの心配りすることが必要です。

ぷるぷる震える現象はkinectメニューの「motion平滑化」を実行すると滑らかになります。

####### 2012/03/14 5:50 プログラム修正。
　　どんな姿勢のモデルデータでもKinectでモーションキャプチャ出来るようになりました。
　　キャプチャ前にIKなどでせんだみつおポーズにし、kinect-->初期姿勢設定メニューを実行すると可能になります。
　　kinectの初期姿勢は現在保存されません。モデルデータを読み直すたびに実行してください。

####### 2012/03/15 7:20 プログラム修正
　　タイムラインの再生ボタン類の大きさを大きくしました。


ソースは下のページからダウンロードしてください。
OpenRDBダウンロードページ



１１−０１、OpenNIのインストール
上記のリンクからOpenRDBで使用するOpenNIをダウンロードできます。
昔のバージョンと比べてインストールは簡単になっています。
まず、古いバージョンのOpenNIがインストールされている場合は
全てアンインストールします。

kinectをパソコンに接続します。
openni-win32-1.5.2.23-dev.msiをダブルクリックします。
画面の指示に従い、設定項目はデフォルトのままインストールします。
nite-win32-1.5.2.21-dev.msiをダブルクリックします。
画面の指示に従い、設定項目はデフォルトのままインストールします。
avin2-SensorKinect-faf4994.zipを解凍します。
そしてBinフォルダの中のSensorKinect091-Bin-Win32-v5.1.0.25.msiを実行します。
パソコンを再起動します。

これでインストールは完了です。
注意点は６４ビットOSでも３２ビット版のプログラムをインストールすることです。


１１−０２、OpenNIでボーンの位置を取得する
OpenNIの関数を使うプログラムはKinecoDXソリューションにまとめてあります。
OpenNIの関数を使う部分を別DLL、KinecoDX.dllにします。
OpenNIのライセンスはGPLなので
この部分を別DLLにすることで他の部分のライセンスに影響を与えないようにします。

OpenNIのボーンは１５個あります。

enum {

	SKEL_TOPOFJOINT,

	SKEL_TORSO,

	SKEL_LEFT_HIP,

	SKEL_LEFT_KNEE,

	SKEL_LEFT_FOOT,

	SKEL_RIGHT_HIP,

	SKEL_RIGHT_KNEE,

	SKEL_RIGHT_FOOT,

	SKEL_NECK,

	SKEL_HEAD,

	SKEL_LEFT_SHOULDER,

	SKEL_LEFT_ELBOW,

	SKEL_LEFT_HAND,

	SKEL_RIGHT_SHOULDER,

	SKEL_RIGHT_ELBOW,

	SKEL_RIGHT_HAND,

	SKEL_MAX

};

上のようなenumを定義しました。
KinecoDXで全部のボーンの３Dの位置を取得します。
位置の配列はSKEL_MAXの要素数で定義します。

SKEL_TOPOFJOINTはこちらで勝手に付け足したものです。
ですので全部で１６個のボーンになります。

OpenNIのおかげでだいぶ簡単に３D座標が取得できます。
KinecoDX.cppを見ればだいたい理解できるでしょう。

ちょっと迷うかもしれないのは
トラッキングの部分です。

OpenNIは初期姿勢としてせんだみつおポーズを要求します。
このポーズを認識するとTrackingFフラグがtrueになり、
ボーン座標の取得が始まります。
TrackingFフラグはDLL外部からはOpenNIIsTracking命令で取得可能です。

トラッキング開始直後はすこし不安定なことがあるので
４回トラッキング成功を認識してからTrackingFフラグをtrueにしています。


１１−０３、kbsファイル
11-02でKinectのボーンの種類を見ました。
実際のモデルデータとKinectのボーンと名前も構造も完全に一致する場合はいいのですが
多くの場合は実際のモデルデータの方がボーンの数が多いですし
名前も違うと思います。

そこで実際のモデルのボーンとKinectのボーンとの対応表を作成します。
OpenRDBではこの対応表をkbsと呼びます。
kbsとは、KinectBoneSettingの略です。

OpenRDBのkinectメニューのkbs作成を実行すると図１１−０１のようなダイアログが出ます。


図１１−０１、kbsダイアログ

図１１−０１のようにこれでボーンの数や名前の違いなどを吸収します。
階層構造は完全に一致する必要はありませんが
kinectで親の方の階層のものが、実際のモデルで子供の方の階層になっていたりすると
うまく動かないので注意が必要です。
つまり階層の上下関係は満たしていないといけません。

このkbsの設定はCModelのCKbsFile*に格納します。

保存も出来ます。
参考までにkbsの一例を載せておきます。

＜?xml version="1.0" encoding="Shift_JIS"?＞

＜KBS＞

  ＜FileInfo＞

    ＜kind＞KinectBoneSettingFile＜/kind＞

    ＜version＞1001＜/version＞

    ＜type＞0＜/type＞

  ＜/FileInfo＞

  ＜TOP_OF_JOINT＞

    ＜RDBBone＞ModelRootBone＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/TOP_OF_JOINT＞

  ＜SKEL_TORSO＞

    ＜RDBBone＞Base_X＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_TORSO＞

  ＜SKEL_LEFT_HIP＞

    ＜RDBBone＞FLOAT_Foot1[L]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_LEFT_HIP＞

  ＜SKEL_LEFT_KNEE＞

    ＜RDBBone＞Foot1[L]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_LEFT_KNEE＞

  ＜SKEL_LEFT_FOOT＞

    ＜RDBBone＞Foot2[L]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_LEFT_FOOT＞

  ＜SKEL_RIGHT_HIP＞

    ＜RDBBone＞FLOAT_Foot1[R]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_RIGHT_HIP＞

  ＜SKEL_RIGHT_KNEE＞

    ＜RDBBone＞Foot1[R]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_RIGHT_KNEE＞

  ＜SKEL_RIGHT_FOOT＞

    ＜RDBBone＞Foot2[R]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_RIGHT_FOOT＞

  ＜SKEL_NECK＞

    ＜RDBBone＞Neck_X＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_NECK＞

  ＜SKEL_HEAD＞

    ＜RDBBone＞HeadSaki_X＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_HEAD＞

  ＜SKEL_LEFT_SHOULDER＞

    ＜RDBBone＞FLOAT_Arm0[L]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_LEFT_SHOULDER＞

  ＜SKEL_LEFT_ELBOW＞

    ＜RDBBone＞Arm0[L]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_LEFT_ELBOW＞

  ＜SKEL_LEFT_HAND＞

    ＜RDBBone＞Arm1[L]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_LEFT_HAND＞

  ＜SKEL_RIGHT_SHOULDER＞

    ＜RDBBone＞FLOAT_Arm0[R]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_RIGHT_SHOULDER＞

  ＜SKEL_RIGHT_ELBOW＞

    ＜RDBBone＞Arm0[R]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_RIGHT_ELBOW＞

  ＜SKEL_RIGHT_HAND＞

    ＜RDBBone＞Arm1[R]＜/RDBBone＞

    ＜twist＞1＜/twist＞

  ＜/SKEL_RIGHT_HAND＞

＜/KBS＞



全角で表示していますが、実際のファイルでは＜＞は半角です。



１１−０４、初期姿勢登録

OpenNIでは初期姿勢がせんだみつおぽーずであることが必須ですが
このポーズでモデリングすると影響度の設定がうまくいかないことがあります。
ですのでモデリングのポーズを自由に出来る工夫をしました。

モデルデータを読み込んだ後に、IKなどでせんだみつおのぽーずにします。
そしてkinectメニューの「初期姿勢登録」を実行します。

登録実行時にせんだみつおポーズにするための各ボーンのクォータニオンを
CBone::m_kinectqに格納します。

モーションキャプチャはせんだみつおの初期ポーズを想定して計算して
最後に実際のモデルにクォータニオンをセットする際に
m_kinectqで回転してからモーションキャプチャのクォータニオンで回転するような
クォータニオンの掛け算をして設定します。

こうすることでどんなポーズでモデリングしても
OpenNIでモーションキャプチャできるようになります。


１１−０５、kinectモーションキャプチャ

それではいよいよモーションキャプチャのソースを見てみます。
モーションキャプチャのメイン呼び出しはKinectMain.cppにあります。

キャプチャがスタートしたら毎フレームCKinectMain::Updateを呼び出します。
Updateのソースを貼り付けます。

int CKinectMain::Update( CModel* srcmodel, double* dstframe )

{

  if( (m_validflag == 0) || !m_model->m_kbs || !m_model->m_rps ){

    _ASSERT( 0 );

    *dstframe = 0.0;

    return 0;

  }

  if( m_rendercnt % 2 ){

    m_rendercnt++;

    return 0;

  }





  OpenNIDrawDepthMap( 0 );



  bool trflag = false;

  OpenNIIsTracking( &trflag );



  D3DXVECTOR3 tmppos[ SKEL_MAX ];

  int ret;





  if( (m_rendercnt == 0) && trflag ){

    m_capframe = m_capstartframe;



    ret = m_model->m_rps->InitArMp( m_model->m_kbs->m_elem, m_capmotid, m_capframe );

    _ASSERT( !ret );

		

    OpenNIGetSkeltonJointPosition( SKEL_MAX, tmppos );

    int skno;

    for( skno = 0; skno < SKEL_MAX; skno++ ){

      tmppos[skno].x *= -1.0f;

    }

    ret = m_model->m_rps->SetRpsElem( 0, tmppos );

    _ASSERT( !ret );



    ret = m_model->m_rps->CalcTraQ( m_model->m_kbs->m_elem, 1 );

    _ASSERT( !ret );



    MODELBOUND mb;

    m_model->GetModelBound( &mb );

    ret = m_model->m_rps->SetMotion( mb, m_model->m_kbs->m_elem, m_capmotid, m_capframe );

    _ASSERT( !ret );



    m_rendercnt++;

  }else if( trflag ){



    int dosetflag = 0;



    if( m_capmode == 0 ){

      m_capframe = (double)m_rendercnt / 2.0 * m_capstep;

      if( m_capframe > m_capendframe ){

        m_rendercnt = 0;

        *dstframe = m_capendframe;

        EndCapture();

        return 2;//!!!!!!!!!!!!!!!!!終わりの印

      }

      dosetflag = 1;

    }



    OpenNIGetSkeltonJointPosition( SKEL_MAX, tmppos );

    int skno;

    for( skno = 0; skno < SKEL_MAX; skno++ ){

      tmppos[skno].x *= -1.0f;

    }

    ret = m_model->m_rps->SetRpsElem( 2, tmppos );

    _ASSERT( !ret );



    ret = m_model->m_rps->CalcTraQ( m_model->m_kbs->m_elem, 1 );

    _ASSERT( !ret );



    if( dosetflag == 1 ){

      MODELBOUND mb;

      m_model->GetModelBound( &mb );

      ret = m_model->m_rps->SetMotion( mb, m_model->m_kbs->m_elem, m_capmotid, m_capframe );

      _ASSERT( !ret );

    }

    m_rendercnt++;

  }else{

    m_rendercnt = 0;

    *dstframe = 0.0;

    return 0;//!!!!!!!!!!!!!!!!!!!!!!!

  }



  *dstframe = m_capframe;



  return 0;

}


冒頭でm_rendercnt % 2が０以外の時にリターンしています。
これはOpenRDBのfpsが６０なのに対し、OpenNIのfpsが３０なためです。

トラッキングがtrueのときのみボーン位置の計算をします。
ボーンの位置はCRpsに格納します。
rpsとはリアルタイムポジションの略です。
CRpsでは３フレーム分、全ボーンの位置を保持します。
最初のフレーム、フレーム０にはキャプチャを開始してトラッキングがオンになった直後の
ボーンの位置を保存します。
これはせんだみつおポーズの座標で、全ポーズ計算の基準値となります。

最後のフレーム２には最新のボーン位置を格納します。
そして真ん中のフレーム１には１回前の計算時のボーンの位置を格納します。

この座標セットの部分がCRps::SetRpsElemです。

int CRps::SetRpsElem( int frameno, D3DXVECTOR3* srcpos )

{

  if( frameno == 0 ){

    int fno;

    for( fno = 0; fno < 3; fno++ ){

      int skno;

      for( skno = 0; skno < SKEL_MAX; skno++ ){

        RPSELEM* curelem;

        curelem = m_pelem + fno * SKEL_MAX + skno;



        curelem->pos = *( srcpos + skno );

        curelem->skelno = skno;

        curelem->framecnt = fno;

        curelem->confidence = 1.0f;

        curelem->twistflag = 1;



        CTraQ* curtq = m_traq + fno * SKEL_MAX + skno;

        curtq->InitParams();

      }

    }

  }else{

    int skno;



    if( frameno == 2 ){

      MoveMemory( m_pelem + 1 * SKEL_MAX, m_pelem + 2 * SKEL_MAX, sizeof( RPSELEM ) * SKEL_MAX );

      MoveMemory( m_traq + 1 * SKEL_MAX, m_traq + 2 * SKEL_MAX, sizeof( CTraQ ) * SKEL_MAX );

      for( skno = 0; skno < SKEL_MAX; skno++ ){

        ( m_traq + 2 * SKEL_MAX + skno )->InitParams();

      }

    }



    for( skno = 0; skno < SKEL_MAX; skno++ ){

      RPSELEM* curelem;

      curelem = m_pelem + frameno * SKEL_MAX + skno;



      curelem->pos = *( srcpos + skno );// - (m_pelem + 0 * SKEL_MAX + SKEL_TORSO)->pos;

      curelem->skelno = skno;

      curelem->framecnt = frameno;

      curelem->confidence = 1.0f;

      curelem->twistflag = 1;

    }

  }

    return 0;

}

上で説明したとおり、３フレーム分のボーン位置の設定をしています。

クォータニオンを計算するだけならフレーム０とフレーム２だけで出来ます。
１回前のポジションを保持する理由はオイラー角の計算のためです。
クォータニオンをオイラー角に直す場合
オイラー角は何通りも考えられます。
これを一意に絞り込むために１回前の姿勢をチェックし、それに一番近いオイラー角を計算します。


CKinectMain::Updateでは位置を設定した後にCalcTraQを呼び出しています。
CalcTraQでボーン位置からボーンのローカルのクォータニオンを計算します。

CRps::CalcTraQのソースを貼り付けます。

int CRps::CalcTraQ( TSELEM* tsptr, int chksym )

{

  if( !m_pelem || !m_traq ){

    _ASSERT( 0 );

    return 1;

  }



  int ret;

  ret = SetSkipFlag();

  _ASSERT( !ret );





  int frameno = 2;



  ret = m_traq->CalcTorso( m_traq, m_pelem, frameno, m_skipflag[SKEL_TORSO] );

  ret = m_traq->CalcNeck( m_model, tsptr, m_traq, m_pelem, frameno, m_skipflag[SKEL_NECK] );



  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_TORSO, SKEL_LEFT_HIP, 3 );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_LEFT_HIP, SKEL_LEFT_KNEE, m_skipflag[SKEL_LEFT_KNEE] );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_LEFT_KNEE, SKEL_LEFT_FOOT, m_skipflag[SKEL_LEFT_FOOT] );



  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_TORSO, SKEL_RIGHT_HIP, 3 );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_RIGHT_HIP, SKEL_RIGHT_KNEE, m_skipflag[SKEL_RIGHT_KNEE] );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_RIGHT_KNEE, SKEL_RIGHT_FOOT, m_skipflag[SKEL_RIGHT_FOOT] );



  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_NECK, SKEL_HEAD, m_skipflag[SKEL_HEAD] );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_NECK, SKEL_LEFT_SHOULDER, 3 );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_LEFT_SHOULDER, SKEL_LEFT_ELBOW, m_skipflag[SKEL_LEFT_ELBOW] );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_LEFT_ELBOW, SKEL_LEFT_HAND, m_skipflag[SKEL_LEFT_HAND] );



  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_NECK, SKEL_RIGHT_SHOULDER, 3 );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_RIGHT_SHOULDER, SKEL_RIGHT_ELBOW, m_skipflag[SKEL_RIGHT_ELBOW] );

  ret = m_traq->CalcQ( m_model, tsptr, m_traq, m_pelem, frameno, SKEL_RIGHT_ELBOW, SKEL_RIGHT_HAND, m_skipflag[SKEL_RIGHT_HAND] );



	//sym

  if( chksym == 1 ){

    ret = SetSym( SKEL_TOPOFJOINT, SKEL_TOPOFJOINT, tsptr, m_model );

    ret = SetSym( SKEL_TORSO, SKEL_TORSO, tsptr, m_model );



    ret = SetSym( SKEL_LEFT_HIP, SKEL_RIGHT_HIP, tsptr, m_model );

    ret = SetSym( SKEL_LEFT_KNEE, SKEL_RIGHT_KNEE, tsptr, m_model );

    ret = SetSym( SKEL_LEFT_FOOT, SKEL_RIGHT_FOOT, tsptr, m_model );



    ret = SetSym( SKEL_NECK, SKEL_NECK, tsptr, m_model );

    ret = SetSym( SKEL_HEAD, SKEL_HEAD, tsptr, m_model );



    ret = SetSym( SKEL_LEFT_SHOULDER, SKEL_RIGHT_SHOULDER, tsptr, m_model );

    ret = SetSym( SKEL_LEFT_ELBOW, SKEL_RIGHT_ELBOW, tsptr, m_model );

    ret = SetSym( SKEL_LEFT_HAND, SKEL_RIGHT_HAND, tsptr, m_model );

  }



  //m_q --> m_finalqへコピー

  for( int frameno = 0; frameno < 3; frameno++ ){

    for( int skelno = 0; skelno < SKEL_MAX; skelno++ ){

      (m_traq + frameno * SKEL_MAX + skelno)->m_finalq = (m_traq + frameno * SKEL_MAX + skelno)->m_q;

      (m_traq + frameno * SKEL_MAX + skelno)->m_finaltra = (m_traq + frameno * SKEL_MAX + skelno)->m_tra;

      (m_traq + frameno * SKEL_MAX + skelno)->m_finaleul = (m_traq + frameno * SKEL_MAX + skelno)->m_cureul;



      (m_traq + frameno * SKEL_MAX + skelno)->m_orgq = (m_traq + frameno * SKEL_MAX + skelno)->m_q;

      (m_traq + frameno * SKEL_MAX + skelno)->m_orgtra = (m_traq + frameno * SKEL_MAX + skelno)->m_tra;

      (m_traq + frameno * SKEL_MAX + skelno)->m_orgeul = (m_traq + frameno * SKEL_MAX + skelno)->m_cureul;

    }

  }

  return 0;

}


CTraQクラスでボーン位置からクォータニオンへの変換をします。
この変換はボーンの部位で３種類の計算に分けています。
TorsoはCalcTorso、NeckはCalcNeck、それ以外はCalcQで計算します。

まずはCalcTorsoを見てみます。

int CTraQ::CalcTorso( CTraQ* traqptr, RPSELEM* rpsptr, int frameno,
      int skipflag )

{



  D3DXVECTOR3 befpos, aftpos;

  if( (skipflag & 1) == 0 ){

    befpos = ( rpsptr + 0 * SKEL_MAX + SKEL_TORSO )->pos;

    aftpos = ( rpsptr + frameno * SKEL_MAX + SKEL_TORSO )->pos;



    D3DXVECTOR3 befhip, afthip;

    befhip = ( rpsptr + 0 * SKEL_MAX + SKEL_LEFT_HIP )->pos - ( rpsptr + 0 * SKEL_MAX + SKEL_RIGHT_HIP )->pos;

    afthip = ( rpsptr + frameno * SKEL_MAX + SKEL_LEFT_HIP )->pos - ( rpsptr + frameno * SKEL_MAX + SKEL_RIGHT_HIP )->pos;

    befhip.y = 0.0f;

    afthip.y = 0.0f;

    D3DXVECTOR3 nbefhip, nafthip;

    DVec3Normalize( &nbefhip, befhip );

    DVec3Normalize( &nafthip, afthip );



    double radxz;

    D3DXVECTOR3 vDir0, vDir;

    D3DXVECTOR3 dirY( 0.0f, 1.0f, 0.0f );

    DVec3Cross( &nafthip, &dirY, &vDir0 );

    DVec3Normalize( &vDir, vDir0 );

    if( vDir.x == 0.0f ){

      if( vDir.z >= 0.0f )

        radxz = 0.0;

      else

        radxz = PAI;

    }else if( vDir.x > 0.0f ){

      radxz = -atanf( vDir.z / vDir.x ) + PAI / 2;

    }else{

      radxz = -atanf( vDir.z / vDir.x ) - PAI / 2;

    }



    D3DXVECTOR3 axis( 0.0f, 1.0f, 0.0f );

    CQuaternion qxz;

    qxz.SetAxisAndRot( axis, radxz );



    ( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_q = qxz;

    ( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_totalq = qxz;

    ( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_cureul = D3DXVECTOR3( 0.0f, (float)( radxz * PAI2DEG ), 0.0f );

  }



/////////

  if( skipflag != 3 ){

    D3DXVECTOR3 difftorso;

    difftorso = aftpos - befpos;

    ( traqptr + frameno * SKEL_MAX + SKEL_TOPOFJOINT )->m_tra = difftorso;

  }



  return 0;

}


左右のヒップのベクトルの傾きを計算してそれをクォータニオンにしているだけです。

CalcNeckも貼り付けます。

int CTraQ::CalcNeck( CModel* srcmodel, TSELEM* tsptr, CTraQ* traqptr,
      RPSELEM* rpsptr, int frameno, int skipflag )

{

  CQuaternion parq;

  parq = ( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_totalq;

  CQuaternion invparq;

  parq.inv( &invparq );



  if( skipflag == 3 ){

    ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_q.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_totalq = parq;

    return 0;

  }





  D3DXVECTOR3 befLpos, aftLpos, befRpos, aftRpos;

  befLpos = ( rpsptr + 0 * SKEL_MAX + SKEL_LEFT_SHOULDER )->pos;

  aftLpos = ( rpsptr + frameno * SKEL_MAX + SKEL_LEFT_SHOULDER )->pos;

  befRpos = ( rpsptr + 0 * SKEL_MAX + SKEL_RIGHT_SHOULDER )->pos;

  aftRpos = ( rpsptr + frameno * SKEL_MAX + SKEL_RIGHT_SHOULDER )->pos;



  D3DXVECTOR3 paftLpos, paftRpos;

  invparq.Rotate( &paftLpos, aftLpos );

  invparq.Rotate( &paftRpos, aftRpos );



  double radxz;

  D3DXVECTOR3 vDir0, vDir;

  D3DXVECTOR3 dirY( 0.0f, 1.0f, 0.0f );

  D3DXVECTOR3 diffpos;

  diffpos = paftLpos - paftRpos;

  DVec3Normalize( &diffpos, diffpos );



  DVec3Cross( &diffpos, &dirY, &vDir0 );

  DVec3Normalize( &vDir, vDir0 );

  if( vDir.x == 0.0f ){

    if( vDir.z >= 0.0f )

      radxz = 0.0;

    else

      radxz = PAI;

  }else if( vDir.x > 0.0f ){

    radxz = -atanf( vDir.z / vDir.x ) + PAI / 2;

  }else{

    radxz = -atanf( vDir.z / vDir.x ) - PAI / 2;

  }



///////////////

  D3DXVECTOR3 beftor, afttor, befneck, aftneck;

  beftor = ( rpsptr + 0 * SKEL_MAX + SKEL_TORSO )->pos;

  afttor = ( rpsptr + frameno * SKEL_MAX + SKEL_TORSO )->pos;

  befneck = ( rpsptr + 0 * SKEL_MAX + SKEL_NECK )->pos;

  aftneck = ( rpsptr + frameno * SKEL_MAX + SKEL_NECK )->pos;



  D3DXVECTOR3 pafttor, paftneck;

  invparq.Rotate( &pafttor, afttor );

  invparq.Rotate( &paftneck, aftneck );



  D3DXVECTOR3 vecA, vecB;

  vecA = befneck - beftor;

  vecB = paftneck - pafttor;

  D3DXVECTOR3 nvecA, nvecB;

  DVec3Normalize( &nvecA, vecA );

  DVec3Normalize( &nvecB, vecB );

  int ret;

  CQuaternion q2;

  ret = DCalcDiffQ( &nvecA, &nvecB, &q2 );

  _ASSERT( !ret );

	



  CQuaternion za3q;

  D3DXVECTOR3 cureul( 0.0f, 0.0f, 0.0f );

  D3DXVECTOR3 befeul( 0.0f, 0.0f, 0.0f );

  D3DXVECTOR3 neckbefeul( 0.0f, 0.0f, 0.0f );

  neckbefeul = ( traqptr + ( frameno - 1 ) * SKEL_MAX + SKEL_NECK )->m_neckbefeul;

  ret = QtoEul( srcmodel, q2, neckbefeul, CAX_INI, 0, &cureul, &za3q );

  _ASSERT( !ret );



  CQuaternion invza3q;

  za3q.inv( &invza3q );



  D3DXVECTOR3 dirx( 1.0f, 0.0f, 0.0f );

  D3DXVECTOR3 diry( 0.0f, 1.0f, 0.0f );

  D3DXVECTOR3 dirz( 0.0f, 0.0f, 1.0f );



  CQuaternion qx2, qy2, qz2, qzx2;

  qx2.SetAxisAndRot( dirx, cureul.x * DEG2PAI );

  qy2.SetAxisAndRot( diry, cureul.y * DEG2PAI );

  qz2.SetAxisAndRot( dirz, cureul.z * DEG2PAI );



  CQuaternion q;

  q = qx2 * qz2;





  D3DXVECTOR3 axis;

  q.Rotate( &axis, diry );

  DVec3Normalize( &axis, axis );

  CQuaternion qxz;

  qxz.SetAxisAndRot( axis, radxz );



  CQuaternion setq;

  if( skipflag & 1 ){

    setq = q;

  }else{

    setq = qxz * q;

  }



/////////////////////

  D3DXVECTOR3 setcureul;

  befeul = ( traqptr + ( frameno - 1 ) * SKEL_MAX + SKEL_NECK )->m_befeul;



  ret = QtoEul( srcmodel, setq, befeul, CAX_INI, 0, &setcureul, &za3q );

  _ASSERT( !ret );





  ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_q = setq;

  ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_totalq = parq * setq;



  ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_neckbefeul = cureul;

  ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_cureul = setcureul;

  ( traqptr + frameno * SKEL_MAX + SKEL_NECK )->m_befeul = setcureul;





  return 0;

}


大きく２つの部分に分かれています。
前半はXZ平面での傾きを計算しています。
そして後半は体の前後の傾きを計算しています。

前半はTorsoの計算方法と同じです。

後半でクォータニオンを算出する際にDCalcDiffQを呼んでいます。
これは回転前のベクトルと回転後のベクトルからクォータニオンを計算するものです。
２つのベクトルの外積から回転軸を求め、内積のacosから回転角度を求めて
それらからクォータニオンを計算しているだけです。

CalcQは以下のようになります。

int CTraQ::CalcQ( CModel* srcmodel, TSELEM* tsptr, CTraQ* traqptr,
      RPSELEM* rpsptr, int frameno, int pivotskel, int skelno, int skipflag )

{

  CQuaternion parq;

  parq = ( traqptr + frameno * SKEL_MAX + pivotskel )->m_totalq;

  CQuaternion invparq;

  parq.inv( &invparq );



  if( skipflag != 0 ){

    ( traqptr + frameno * SKEL_MAX + skelno )->m_q.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    ( traqptr + frameno * SKEL_MAX + skelno )->m_totalq = parq;

    return 0;

  }





  D3DXVECTOR3 befpivpos, aftpivpos, befpos, aftpos;

  befpivpos = ( rpsptr + 0 * SKEL_MAX + pivotskel )->pos;

  aftpivpos = ( rpsptr + frameno * SKEL_MAX + pivotskel )->pos;

  befpos = ( rpsptr + 0 * SKEL_MAX + skelno )->pos;

  aftpos = ( rpsptr + frameno * SKEL_MAX + skelno )->pos;



  D3DXVECTOR3 paftpivpos, paftpos;

  invparq.Rotate( &paftpivpos, aftpivpos );

  invparq.Rotate( &paftpos, aftpos );



  D3DXVECTOR3 vec1, vec2;

  vec1 = befpos - befpivpos;

  vec2 = paftpos - paftpivpos;

  D3DXVECTOR3 nvec1, nvec2;

  DVec3Normalize( &nvec1, vec1 );

  DVec3Normalize( &nvec2, vec2 );



  int ret;

  CQuaternion setq;

  ret = DCalcDiffQ( &nvec1, &nvec2, &setq );

  _ASSERT( !ret );

////////////////



  D3DXVECTOR3 cureul( 0.0f, 0.0f, 0.0f );

  D3DXVECTOR3 befeul( 0.0f, 0.0f, 0.0f );

  befeul = ( traqptr + ( frameno - 1 ) * SKEL_MAX + skelno )->m_befeul;

  CQuaternion za3q;

  ret = QtoEul( srcmodel, setq, befeul, CAX_ZA3, (tsptr + skelno)->jointno, &cureul, &za3q );

  _ASSERT( !ret );



  CQuaternion invza3q;

  za3q.inv( &invza3q );



  float twistdeg;

  if( ( tsptr + skelno )->twistflag == 0 ){

    D3DXVECTOR3 dirx( 1.0f, 0.0f, 0.0f );

    D3DXVECTOR3 diry( 0.0f, 1.0f, 0.0f );

    D3DXVECTOR3 dirz( 0.0f, 0.0f, 1.0f );



    CQuaternion qx, qy, qz;

    qx.SetAxisAndRot( dirx, cureul.x * DEG2PAI );

    qy.SetAxisAndRot( diry, cureul.y * DEG2PAI );

    qz.SetAxisAndRot( dirz, cureul.z * DEG2PAI );

    CQuaternion qxy;

    qxy = qy * qx;



///////////		

    D3DXVECTOR3 aftdiry;

    qxy.Rotate( &aftdiry, diry );

    DVec3Normalize( &aftdiry, aftdiry );



    double doty;

    DCalcDot( &diry, &aftdiry, &doty );

    double rady;

    rady = acos( doty );



    CQuaternion ya, yb;

    ya.SetAxisAndRot( dirz, rady );

    yb.SetAxisAndRot( dirz, -rady );

    D3DXVECTOR3 afta, aftb;

    ya.Rotate( &afta, diry );

    yb.Rotate( &aftb, diry );

    DVec3Normalize( &afta, afta );

    DVec3Normalize( &aftb, aftb );

    double dotya, dotyb;

    DCalcDot( &aftdiry, &afta, &dotya );

    DCalcDot( &aftdiry, &aftb, &dotyb );

    CQuaternion twistq;

    if( dotya >= dotyb ){

      twistq = ya;

      twistdeg = (float)( rady * PAI2DEG );

    }else{

      twistq = yb;

      twistdeg = (float)( -rady * PAI2DEG );

    }

		

    setq = za3q * qxy * twistq * invza3q;

  }else{

    twistdeg = cureul.z;

  }





  ( traqptr + frameno * SKEL_MAX + skelno )->m_q = setq;

  ( traqptr + frameno * SKEL_MAX + skelno )->m_totalq = parq * setq;

  ( traqptr + frameno * SKEL_MAX + skelno )->m_befeul = cureul;

  ( traqptr + frameno * SKEL_MAX + skelno )->m_cureul = D3DXVECTOR3( cureul.x, cureul.y, twistdeg );

  return 0;

}


Neckの後半部分と同じです。
回転前後のベクトルからクォータニオンを求めています。
長々とtwistflagが０、つまりねじれ防止機能がオンの時のソースが書いてありますが
これはまだ実験段階で実用的ではありません。

最初にRokDeBone2で作ったテストデータでこのねじれ防止がうまく働いたので
残してありますが、どうやらデータ依存の部分が残ってしまったようで
まだまだ汎用ではありません。

今のところ、この部分は無視してしまって構わないです。

CKinectMain::Updateに戻ります。
クォータニオンを計算したらCRps::SetMotionを呼び出して
モデルデータのモーションポイントにセットします。


int CRps::SetMotion( MODELBOUND srcmb, TSELEM* tsptr, int cookie,
      double setframe )

{

  int frameno = 0;

  if( cookie < 0 ){

    return 0;

  }



  int skelno;

  for( skelno = 0; skelno < SKEL_MAX; skelno++ ){

    CTraQ* curtraq = (m_traq + 2 * SKEL_MAX + skelno);

    int boneno;

    boneno = (tsptr + skelno)->jointno;

    CBone* curbone = m_model->m_bonelist[ boneno ];

    _ASSERT( curbone );



    if( curtraq->m_outputflag == 1 ){

      CQuaternion curq;

      curq = curtraq->m_finalq;

      CMotionPoint* mpptr = 0;

      CMotionPoint* nextptr = 0;

      int existflag = 0;

      curbone->GetBefNextMP( cookie, setframe, &mpptr, &nextptr, &existflag );

      _ASSERT( existflag );

      if( mpptr ){

        if( skelno == SKEL_TOPOFJOINT ){

          D3DXVECTOR3 settra;

          settra = curtraq->m_finaltra;

          float modelh = srcmb.max.y - srcmb.min.y;

          float trascale = modelh / 3000.0f;

          mpptr->m_tra = settra * trascale;

        }

        CQuaternion setq = curtraq->m_finalq * curbone->m_kinectq;

        mpptr->SetQ( &curbone->m_axisq, setq );



      }

    }

  }



  m_model->SetMotionFrame( setframe );



  return 0;

}


SKEL_TOPOFJOINTにだけ移動成分を設定しています。
この際にモデルのスケールとKinectの座標のスケールを合わせるために
モデルのバウンダリー情報を使って適切な値になるようにスケールします。



オープンソースのトップに戻る

トップページに戻る