０７、IK(回転、移動)



第７章ではマウスドラッグでボーンの姿勢を編集できるようにします。

まず座標系を導入しました。
ボーンの軸がZ軸と一致するような座標軸を導入し、それが視覚で分かるようにマニピュレータを表示します。
赤がX軸、緑がY軸、青がZ軸を示します。

マニピュレータを右ドラッグすることによりIKの計算を行い、ボーンの姿勢を編集します。
メインウインドウの「回転IK」「移動IK」のラジオボタンで回転か移動かを選びます。

回転の場合はラジオボタンの下の数字の表示のあるコンボボックスで計算階層数を指定します。
計算階層数は最大値であり、最大値に達する前に浮動ボーンや一番親に達した場合はそこで計算は止まります。
計算階層数００を選んだ場合は、浮動ボーンまたは一番親にたどり着くまで自動でさかのぼって計算します。

マニピュレータの中央の黄色をドラッグすると
自由な軸による回転をします。
赤、緑、青の部分をドラッグするとそれぞれ対応するX,Y,Z軸を回転軸にして回転します。

回転のIKはCCD方式です。




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




０７−０１、ボーン座標系の導入

今までの座標系はグローバル座標系でした。
しかし実際の姿勢の編集時にはボーンの軸の座標系で編集した方が効率がいいです。
そこでまずボーン座標系を導入します。

ボーンの軸がZ軸と一致し、後の２軸と垂直になるような座標系を考えます。
データとしてはこのような変換をするためのクォータニオンCBone::m_axisqを保持します。

CBone::m_axisqの計算方法は以下のようになります。

int CBone::CalcAxisQ()

{

  if( !m_faceptr ){

    m_axisq.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    return 0;

  }

  if( ((m_faceptr->m_bonetype != MIKOBONE_NORMAL) && (m_faceptr->m_bonetype != MIKOBONE_FLOAT)) || 

    (m_vertpos[BT_CHILD] == m_vertpos[BT_PARENT]) ){	

    if( m_parent ){

      m_axisq = m_parent->m_axisq;

    }else{

      m_axisq.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    }

    return 0;

  }



  D3DXVECTOR3 upvec;

  D3DXVECTOR3 vecx1, vecy1, vecz1;



  D3DXVECTOR3 bonevec;

  bonevec = m_vertpos[BT_CHILD] - m_vertpos[BT_PARENT];

  D3DXVec3Normalize( &bonevec, &bonevec );



  if( m_faceptr->m_bonetype == MIKOBONE_FLOAT ){

    if( (bonevec.x != 0.0f) || (bonevec.y != 0.0f) ){

      upvec.x = 0.0f;

      upvec.y = 0.0f;

      upvec.z = 1.0f;

    }else{

      upvec.x = 1.0f;

      upvec.y = 0.0f;

      upvec.z = 0.0f;

    }



    vecz1 = bonevec;

		

    D3DXVec3Cross( &vecx1, &upvec, &vecz1 );

    D3DXVec3Normalize( &vecx1, &vecx1 );



    D3DXVec3Cross( &vecy1, &vecz1, &vecx1 );

    D3DXVec3Normalize( &vecy1, &vecy1 );



  }else if( m_faceptr->m_bonetype == MIKOBONE_NORMAL ){

    vecz1 = bonevec;

    D3DXVECTOR3 hvec;

    hvec = m_vertpos[BT_3RD] - m_vertpos[BT_PARENT];

    D3DXVec3Cross( &vecy1, &bonevec, &hvec );

    D3DXVec3Normalize( &vecy1, &vecy1 );



    D3DXVec3Cross( &vecx1, &vecy1, &vecz1 );

    D3DXVec3Normalize( &vecx1, &vecx1 );

  }else{

    _ASSERT( 0 );

    m_axisq.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    return 1;

  }



  D3DXMATRIX tmpxmat;

  D3DXQUATERNION tmpxq;



  D3DXMatrixIdentity( &tmpxmat );

  tmpxmat._11 = vecx1.x;

  tmpxmat._12 = vecx1.y;

  tmpxmat._13 = vecx1.z;



  tmpxmat._21 = vecy1.x;

  tmpxmat._22 = vecy1.y;

  tmpxmat._23 = vecy1.z;



  tmpxmat._31 = vecz1.x;

  tmpxmat._32 = vecz1.y;

  tmpxmat._33 = vecz1.z;



  D3DXQuaternionRotationMatrix( &tmpxq, &tmpxmat );

  m_axisq.SetParams( tmpxq );



  return 0;

}

Z軸がボーンの軸と一致するようにし、Y軸はmikoto形式の三角ボーンの一番短い辺の頂点の位置を使って計算します。
そしてX,Y,Z軸がすべて垂直の関係になるようにCrossで調整します。

変換行列は新しいX軸が_11, _12, _13成分に、新しいY軸が_21, _22, _23成分に、
新しいZ軸が_31, _32, _33成分となるような回転行列です。
この行列をクォータニオンに変換してCBone::m_axisqに保存します。

座標系を設定したらボーン姿勢のオイラー角の計算も変えないといけません。
修正後のCQuaternion::Q2Eulは以下のようになります。

int CQuaternion::Q2Eul( CQuaternion* axisq, D3DXVECTOR3 befeul, D3DXVECTOR3*
      reteul )

{

  CQuaternion axisQ, invaxisQ, EQ;

  if( axisq ){

    axisQ = *axisq;

    axisQ.inv( &invaxisQ );

    EQ = invaxisQ * *this * axisQ;

  }else{

    axisQ.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    invaxisQ.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    EQ = *this;

  }



  D3DXVECTOR3 Euler;



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

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

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



  D3DXVECTOR3 targetVec, shadowVec;

  D3DXVECTOR3 tmpVec;



  EQ.Rotate( &targetVec, axisZVec );

  shadowVec.x = vecDotVec( &targetVec, &axisXVec );

  shadowVec.y = 0.0f;

  shadowVec.z = vecDotVec( &targetVec, &axisZVec );

  if( lengthVec( &shadowVec ) == 0.0f ){

    Euler.y = 90.0f;

  }else{

    Euler.y = aCos( vecDotVec( &shadowVec, &axisZVec ) / lengthVec( &shadowVec ) );		

  }

  if( vecDotVec( &shadowVec, &axisXVec ) < 0.0f ){

    Euler.y = -Euler.y;

  }



  vec3RotateY( &tmpVec, -Euler.y, &targetVec );

  shadowVec.x = 0.0f;

  shadowVec.y = vecDotVec( &tmpVec, &axisYVec );

  shadowVec.z = vecDotVec( &tmpVec, &axisZVec );

  if( lengthVec( &shadowVec ) == 0.0f ){

    Euler.x = 90.0f;

  }else{

    Euler.x = aCos( vecDotVec( &shadowVec, &axisZVec ) / lengthVec( &shadowVec ) );

  }

  if( vecDotVec( &shadowVec, &axisYVec ) > 0.0f ){

    Euler.x = -Euler.x;

  }





  EQ.Rotate( &targetVec, axisYVec );

  vec3RotateY( &tmpVec, -Euler.y, &targetVec );

  targetVec = tmpVec;

  vec3RotateX( &tmpVec, -Euler.x, &targetVec );

  shadowVec.x = vecDotVec( &tmpVec, &axisXVec );

  shadowVec.y = vecDotVec( &tmpVec, &axisYVec );

  shadowVec.z = 0.0f;

  if( lengthVec( &shadowVec ) == 0.0f ){

    Euler.z = 90.0f;

  }else{

    Euler.z = aCos( vecDotVec( &shadowVec, &axisYVec ) / lengthVec( &shadowVec ) );

  }

  if( vecDotVec( &shadowVec, &axisXVec ) > 0.0f ){

    Euler.z = -Euler.z;

  }



  ModifyEuler( &Euler, &befeul );

  *reteul = Euler;



  return 0;

}

EQ = invaxisQ * *this * axisQ;
というところがミソです。
ボーン座標系でのクォータニオンに直しています。
axisQはCBone::m_axisqでinvaxisQはその逆クォータニオンです。

オイラー角からクォータニオンに直すときにも変更があります。

int CQuaternion::SetRotation( CQuaternion* axisq, D3DXVECTOR3 srcdeg
      )

{

  // Z軸、X軸、Y軸の順番で、回転する、クォータニオンをセットする。



  CQuaternion axisQ, invaxisQ;

  if( axisq ){

    axisQ = *axisq;

    axisQ.inv( &invaxisQ );

  }else{

    axisQ.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

    invaxisQ.SetParams( 1.0f, 0.0f, 0.0f, 0.0f );

  }



  CQuaternion q, qx, qy, qz;

  float cosx, sinx, cosy, siny, cosz, sinz;

  float fDeg2Pai = (float)DEG2PAI;



  cosx = (float)cos( srcdeg.x * 0.5f * fDeg2Pai );

  sinx = (float)sin( srcdeg.x * 0.5f * fDeg2Pai );

  cosy = (float)cos( srcdeg.y * 0.5f * fDeg2Pai );

  siny = (float)sin( srcdeg.y * 0.5f * fDeg2Pai );

  cosz = (float)cos( srcdeg.z * 0.5f * fDeg2Pai );

  sinz = (float)sin( srcdeg.z * 0.5f * fDeg2Pai );



  qx.SetParams( cosx, sinx, 0.0f, 0.0f );

  qy.SetParams( cosy, 0.0f, siny, 0.0f );

  qz.SetParams( cosz, 0.0f, 0.0f, sinz );

  

  q = axisQ * qy * qx * qz * invaxisQ;

  *this = q;



  return 0;

}

q = axisQ * qy * qx * qz * invaxisQ;
というところが修正部分です。

OpenRDBでのクォータニオンの掛け算の記述には注意が必要です。
回転変換の順番は掛け算記述と逆になります。
q1 * q2と書くとq2の回転のあとにq1の回転をするという意味です。

０７−０２、マニピュレータの描画

ボーン座標系に合わせて座標軸の向きをマニピュレータで選択ボーンの位置に表示します。
ボーンの選択は右クリックで行います。
左クリックじゃなくて右クリックなので注意してください。

マニピュレータはRDBMain.cppで作成表示します。
メタセコイアのファイルはselect_2.mqoです。Media/RDBMediaに入っています。
これをCModelであるs_selectに読み込みます。

一工夫いるのはカメラの距離などによってマニピュレータの大きさが変化しないようにすることです。
マニピュレータの描画部分のコードを貼り付けます。

int RenderSelectMark()

{

  D3DXMATRIX mw = g_mCenterWorld * *g_Camera.GetWorldMatrix();

  D3DXMATRIX mp = *g_Camera.GetProjMatrix();

  D3DXMATRIX mv = *g_Camera.GetViewMatrix();

  D3DXMATRIX mvp = mv * mp;



  CBone* curboneptr = s_model->m_bonelist[ s_curboneno ];

  if( curboneptr ){

    D3DXMATRIX selm;

    D3DXMATRIX bonetra;

    selm = curboneptr->m_axisq.MakeRotMatX() * curboneptr->m_curmp.m_worldmat;



    D3DXVECTOR3 orgpos = curboneptr->m_vertpos[BT_CHILD];

    D3DXVECTOR3 bonepos = curboneptr->m_childworld;



    D3DXVECTOR3 cam0, cam1;

    D3DXMATRIX mwv = mw * mv;

    D3DXVec3TransformCoord( &cam0, &orgpos, &mwv );

    cam1 = cam0 + D3DXVECTOR3( 1.0f, 0.0f, 0.0f );



    D3DXVECTOR3 sc0, sc1;

    float selectscale;

    D3DXVec3TransformCoord( &sc0, &cam0, &mp );

    D3DXVec3TransformCoord( &sc1, &cam1, &mp );

    float lineleng = (sc0.x - sc1.x) * (sc0.x - sc1.x) + (sc0.y - sc1.y) * (sc0.y - sc1.y);

    if( lineleng != 0.0f ){

      lineleng = sqrt( lineleng );

      selectscale = 0.0020f / lineleng;

    }else{

      selectscale = 0.0f;

    }



    D3DXMATRIX scalemat;

    D3DXMatrixIdentity( &scalemat );

    scalemat._11 *= selectscale;

    scalemat._22 *= selectscale;

    scalemat._33 *= selectscale;



    D3DXMATRIX selworld;

    selworld = scalemat * selm;

    selworld._41 = bonepos.x;

    selworld._42 = bonepos.y;

    selworld._43 = bonepos.z;



    g_pEffect->SetMatrix( g_hmVP, &mvp );

    g_pEffect->SetMatrix( g_hmWorld, &selworld );

    s_select->UpdateMatrix( -1, &selworld, &mvp );



    s_select->SetShaderConst();

    int lightflag = 1;

    D3DXVECTOR4 diffusemult = D3DXVECTOR4(1.0f, 1.0f, 1.0f, 0.7f);

    s_pdev->SetRenderState( D3DRS_ZFUNC, D3DCMP_ALWAYS );

    s_select->OnRender( s_pdev, lightflag, diffusemult );

    s_pdev->SetRenderState( D3DRS_ZFUNC, D3DCMP_LESSEQUAL );

  }



  return 0;

}

マニピュレータのワールド変換行列を求めるのが少し難しいです。

同じ大きさにするための計算はselectscaleを求める部分です。
まずカメラ座標系でのボーンの位置とカメラ座標系でそこから(1.0, 0.0, 0.0)だけ離れた点を
スクリーン座標に変換します。
そしてスクリーン座標系での長さが一定になるようにselectscaleを計算します。

なぜカメラ座標系で２点を決めたかというと
カメラの向きに関わらず画面上の座標を決定できるからです。

０７−０３、マウスとオブジェクトのあたり判定

マニピュレータの表示が出来たら、それをマウスでクリックしたときに
マニピュレータのどの部分をクリックしたのかを判定しなければなりません。
そのための処理を説明します。

マウスでクリックした情報をRDBMain.cppのs_pickinfoに保持するようにします。
s_pickinfoはCoef.hで定義しているPICKINFO構造体です。

enum {

  PICK_NONE = 0,

  PICK_CENTER = 1,

  PICK_X = 2,

  PICK_Y = 4,

  PICK_Z = 8

};



typedef struct tag_pickinfo

{

  int buttonflag;//WM_LBUTTONDOWN-->PICK_L, WM_RBUTTONDOWN-->PICK_R, WM_MBUTTONDOWN-->PICK_M。押していないとき-->PICK_NONE。

  //以下、buttonflagがPICK_NONE以外の時に意味を持つ。

  POINT mousepos;

  POINT mousebefpos;

  POINT clickpos;

  D3DXVECTOR2 diffmouse;

  int winx;

  int winy;

  int pickrange;

  int pickobjno;

  D3DXVECTOR3 objscreen;

  D3DXVECTOR3 objworld;

}PICKINFO;


PICKINFO.buttonflagにはPICK_*で定義したenumのどれかが設定されます。
PICK_CENTERはボーンをクリックしたときに設定されます。
PICK_X,Y,Zはそれぞれ赤、緑、青の部分をクリックしたときに設定されます。


まずボーンを右クリックしたかどうかの判定方法を説明します。
ボーンのクリック判定はCModel::PickBoneで行います。

int CModel::PickBone( PICKINFO* pickinfo )

{

  pickinfo->pickobjno = -1;



  float fw, fh;

  fw = (float)pickinfo->winx / 2.0f;

  fh = (float)pickinfo->winy / 2.0f;



  int minno = -1;

  D3DXVECTOR3 cmpsc;

  D3DXVECTOR3 picksc = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );

  D3DXVECTOR3 pickworld = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );

  float cmpdist;

  float mindist = 0.0f;

  int firstflag = 1;



  map<int, CBone*>::iterator itrbone;

  for( itrbone = m_bonelist.begin(); itrbone != m_bonelist.end(); itrbone++ ){

    CBone* curbone = itrbone->second;

    if( curbone ){

      cmpsc.x = ( 1.0f + curbone->m_childscreen.x ) * fw;

      cmpsc.y = ( 1.0f - curbone->m_childscreen.y ) * fh;

      cmpsc.z = curbone->m_childscreen.z;



      if( (cmpsc.z >= 0.0f) && (cmpsc.z <= 1.0f) ){

        float mag;

        mag = ( (float)pickinfo->clickpos.x - cmpsc.x ) * ( (float)pickinfo->clickpos.x - cmpsc.x ) + 

          ( (float)pickinfo->clickpos.y - cmpsc.y ) * ( (float)pickinfo->clickpos.y - cmpsc.y );

        if( mag != 0.0f ){

          cmpdist = sqrtf( mag );

        }else{

          cmpdist = 0.0f;

        }



        if( (firstflag || (cmpdist <= mindist)) && (cmpdist <= (float)pickinfo->pickrange ) ){

          minno = curbone->m_boneno;

          mindist = cmpdist;

          picksc = cmpsc;

          pickworld = curbone->m_childworld;

          firstflag = 0;

        }

      }

    }

  }



  pickinfo->pickobjno = minno;

  if( minno >= 0 ){

    pickinfo->objscreen = picksc;

    pickinfo->objworld = pickworld;

  }



  return 0;

}

UpdateMatrixを実行するとCBone::m_childscreenにボーンのスクリーン座標がセットされます。
この値は画面中央が(0, 0)で画面内が-1から1の間に入るような数値です。
マウスでクリックした座標と比べるために、この-1から1の値をウインドウの大きさを考慮した
ウインドウの座標に変換します。

これをしてるのが
cmpsc.x = ( 1.0f + curbone->m_childscreen.x ) * fw;
cmpsc.y = ( 1.0f - curbone->m_childscreen.y ) * fh;
の部分です。

そしてボーンのウインドウ座標とマウスの位置の距離がPICKINFO.pickrangeより小さいボーンのうち
一番距離が小さいものを選びそのボーン番号をPICKINFO.pickobjnoにセットします。


マニピュレータの赤、緑、青の部分とマウスの判定はまたちょっと違います。
これはCModel::CollisionNoBoneObj_Mouseで行います。

int CModel::CollisionNoBoneObj_Mouse( PICKINFO* pickinfo, char* objnameptr
      )

{

  //当たったら１、当たらなかったら０を返す。エラーも０を返す。



  CMQOObject* curobj = m_objectname[ objnameptr ];

  if( !curobj ){

    _ASSERT( 0 );

    return 0;

  }



  D3DXVECTOR3 startlocal, dirlocal;

  CalcMouseLocalRay( pickinfo, &startlocal, &dirlocal );



  int colli = curobj->CollisionLocal_Ray( startlocal, dirlocal );

  return colli;

}

今度は３Dのローカル座標系で判定します。
まずCalcMouseLocalRayでマウス座標を始点と方向のRayに変換します。
そしてそのRayとCMQOObject::CollisionLocal_Rayであたり判定をします。
CollisionLocal_Rayは、アンカーとRayとの判定方法と同じです。

０７−０４、自由軸のIK回転

マニピュレータの中央の黄色い部分を右ドラッグすると自由軸でIK回転するようにします。
IK回転のアルゴリズムにはCCD方式を採用します。
手順としてはまずマウスの位置からドラッグしたボーンの目標位置を求めます。

次にCCD方式に従って、ボーンの目標位置に近づくように子供から親にかけて順番に回転していきます。

この際どこまで親をさかのぼるかを指定できるようにしています。
メインウインドウの右下の数字の書いてあるコンボボックスでさかのぼる階層数を指定します。
００は特別で００を指定した場合は一番親に達するかもしくは浮動ボーンに到達するまでさかのぼります。
００以外でも途中で一番親に達したり浮動ボーンがあった場合はそこでストップします。

親をさかのぼっての回転作業は繰り返すほどボーンが目標地点に近くなります。
しかし回数が多くなると当然重くなるので今回は１回のIKにつき３回計算するようにしています。

ではまず目標座標の計算コードから見てみましょう。
目標座標はRDBMain.cppのCalcTargetPosで計算します。

int CalcTargetPos( D3DXVECTOR3* dstpos )

{

  D3DXVECTOR3 start3d, end3d;

  CalcPickRay( &start3d, &end3d );



  //カメラの面とレイとの交点(targetpos)を求める。

  D3DXVECTOR3 sb, se, n;

  sb = s_pickinfo.objworld - start3d;

  se = end3d - start3d;

  n = *g_Camera.GetLookAtPt() - *g_Camera.GetEyePt();



  float t;

  t = D3DXVec3Dot( &sb, &n ) / D3DXVec3Dot( &se, &n );

  

  *dstpos = ( 1.0f - t ) * start3d + t * end3d;

  return 0;

}



int CalcPickRay( D3DXVECTOR3* startptr, D3DXVECTOR3* endptr )

{

  s_pickinfo.diffmouse.x = (float)( s_pickinfo.mousepos.x - s_pickinfo.mousebefpos.x );

  s_pickinfo.diffmouse.y = (float)( s_pickinfo.mousepos.y - s_pickinfo.mousebefpos.y );



  D3DXVECTOR3 mousesc;

  mousesc.x = s_pickinfo.objscreen.x + s_pickinfo.diffmouse.x;

  mousesc.y = s_pickinfo.objscreen.y + s_pickinfo.diffmouse.y;

  mousesc.z = s_pickinfo.objscreen.z;



  D3DXVECTOR3 startsc, endsc;

  float rayx, rayy;

  rayx = mousesc.x / (s_pickinfo.winx / 2.0f) - 1.0f;

  rayy = 1.0f - mousesc.y / (s_pickinfo.winy / 2.0f);



  startsc = D3DXVECTOR3( rayx, rayy, 0.0f );

  endsc = D3DXVECTOR3( rayx, rayy, 1.0f );

	

  D3DXMATRIX mView;

  D3DXMATRIX mProj;

  mProj = *g_Camera.GetProjMatrix();

  mView = *g_Camera.GetViewMatrix();

  D3DXMATRIX mVP, invmVP;

  mVP = mView * mProj;

  D3DXMatrixInverse( &invmVP, NULL, &mVP );



  D3DXVec3TransformCoord( startptr, &startsc, &invmVP );

  D3DXVec3TransformCoord( endptr, &endsc, &invmVP );



  return 0;

}

CalcPickRayでマウス座標をRayに変換します。
まずスクリーン座標系で計算します。
マウス座標の変化分をボーンのスクリーン座標に足してRayのスクリーン座標のX,Yを求めます。
これがD3DXVECTOR3 mousescです。

次にRayの始点と終点のスクリーン座標startscとendscを求めます。
XとYはmousescと同じです。
スクリーン座標において、見える範囲の一番小さいZの値は０．０、一番大きいZの値は１．０です。
ですのでstartscのZを０．０にし、endscのZを１．０に設定します。
これでRayのスクリーン座標が求まりました。

あとはワールド座標にするためにビュー変換とプロジェクション変換の逆変換をします。
ビューとプロジェクションの変換行列をmVPに求め
この逆行列invVPを求めます。

これをD3DXVec3TransformCoordでスクリーン座標にかけてやると、ワールド座標が求まります。
このようにしてstartとendのワールド座標を求めます。

CalcTargetPosの残りの部分では、このRayとカメラ平面との交点を求め、これを目標座標とします。

Rayの式は
(1-t)OS + t OE
で表せます。

カメラ平面上の一点をボーンの位置Bとし、平面の法線をｎとし、交点をT(target)とすると
平面の式は
BT・ｎ = 0
(・は内積)
で表せます。

TはRay上の点なのでこのTにRayの式を代入してtを求めると
t = SB・ｎ / SE・ｎ
となります。
これをRayの式に代入して
求める交点、つまり目標座標が求まります。



目標座標が求まったらCModel::IKRotateでIK回転処理を行います。

int CModel::IKRotate( int srcboneno, D3DXVECTOR3 targetpos, int maxlevel
      )

{

  m_newmplist.erase( m_newmplist.begin(), m_newmplist.end() );



  CBone* firstbone = m_bonelist[ srcboneno ];

  if( !firstbone ){

    _ASSERT( 0 );

    return 1;

  }



  int calccnt;

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

    int levelcnt = 0;



    CBone* curbone = firstbone;

    while( curbone && ((maxlevel == 0) || (levelcnt < maxlevel)) && 

      ( !curbone->m_faceptr || (curbone->m_faceptr && curbone->m_faceptr->m_bonetype == MIKOBONE_NORMAL) ) )

    {

      if( curbone->m_vertpos[BT_CHILD] != curbone->m_vertpos[BT_PARENT] ){

        D3DXVECTOR3 parworld, chilworld;

        chilworld = firstbone->m_childworld;

        D3DXVec3TransformCoord( &parworld, &(curbone->m_vertpos[BT_PARENT]), &(curbone->m_curmp.m_worldmat) );



        D3DXVECTOR3 parbef, chilbef, tarbef;

        CBone* parbone = curbone->m_parent;

        if( parbone ){

          D3DXMATRIX invmat;

          D3DXMatrixInverse( &invmat, NULL, &(parbone->m_curmp.m_worldmat) );

          D3DXVec3TransformCoord( &parbef, &parworld, &invmat );

          D3DXVec3TransformCoord( &chilbef, &chilworld, &invmat );

          D3DXVec3TransformCoord( &tarbef, &targetpos, &invmat );

        }else{

          parbef = parworld;

          chilbef = chilworld;

          tarbef = targetpos;

        }



        D3DXVECTOR3 vec0, vec1;

        vec0 = chilbef - parbef;

        D3DXVec3Normalize( &vec0, &vec0 );

        vec1 = tarbef - parbef;

        D3DXVec3Normalize( &vec1, &vec1 );



        D3DXVECTOR3 rotaxis;

        D3DXVec3Cross( &rotaxis, &vec0, &vec1 );

        D3DXVec3Normalize( &rotaxis, &rotaxis );



        float rotdot, rotrad;

        rotdot = DXVec3Dot( &vec0, &vec1 );

        rotdot = min( 1.0f, rotdot );

        rotdot = max( -1.0f, rotdot );

        rotrad = (float)acos( rotdot );

        if( rotrad > 1.0e-4 ){

          CQuaternion rotq;

          rotq.SetAxisAndRot( rotaxis, rotrad );

          CMotionPoint* newmp = 0;

          int existflag = 0;

          newmp = curbone->AddMotionPoint( m_curmotinfo->motid, m_curmotinfo->curframe, 1, &existflag );

          CQuaternion q0 = newmp->m_q;

          D3DXVECTOR3 eul0 = newmp->m_eul;



          if( existflag == 0 ){

            NEWMPELEM newmpe;

            newmpe.boneptr = curbone;

            newmpe.mpptr = newmp;

            m_newmplist.push_back( newmpe );

          }



          curbone->MultQ( m_curmotinfo->motid, m_curmotinfo->curframe, rotq );

          UpdateMatrix( curbone->m_boneno, &m_matWorld, &m_matVP );

        }

      }



      curbone = curbone->m_parent;

      levelcnt++;

    }

  }



  return 0;

}

スタンダードなCCD方式のIKです。
詳しく知りたい方は検索すればいくらでも説明は見つかるのでここでは省略します。

ポイントだけ言っておくと
計算は各ボーンの親のボーンの座標系で行うということです。
そのために計算で使う各座標に親のボーンの変換行列の逆行列をかけています。

０７−０５、指定軸のIK回転

マニピュレータの赤、緑、青の部分を右ドラッグした場合は
それぞれボーン座標系のX,Y,Z軸を回転軸とするIKを行います。

この指定軸のIK回転はCModel::IKRotateAxisDeltaで行います。

int CModel::IKRotateAxisDelta( int axiskind, int srcboneno, float
      delta, int maxlevel )

{

  m_newmplist.erase( m_newmplist.begin(), m_newmplist.end() );



  int levelcnt = 0;

  CBone* firstbone = m_bonelist[ srcboneno ];

  if( !firstbone ){

    _ASSERT( 0 );

    return 1;

  }

  D3DXVECTOR3 firstpos = firstbone->m_childworld;



  CBone* calcrootbone = GetCalcRootBone( firstbone, maxlevel );

  _ASSERT( calcrootbone );

  D3DXVECTOR3 rootpos = calcrootbone->m_childworld;



  D3DXVECTOR3 axis0, rotaxis;

  D3DXVECTOR3 targetpos;

  D3DXMATRIX selectmat;

  D3DXMATRIX mat, befrotmat, rotmat, aftrotmat;



  selectmat = firstbone->m_axisq.MakeRotMatX() * firstbone->m_curmp.m_worldmat; 

  selectmat._41 = 0.0f;

  selectmat._42 = 0.0f;

  selectmat._43 = 0.0f;



  float rotrad;

  if( axiskind == PICK_X ){

    axis0 = D3DXVECTOR3( 1.0f, 0.0f, 0.0f );

    D3DXVec3TransformCoord( &rotaxis, &axis0, &selectmat );

    D3DXVec3Normalize( &rotaxis, &rotaxis );

    rotrad = delta / 10.0f * (float)PAI / 12.0f;

  }else if( axiskind == PICK_Y ){

    axis0 = D3DXVECTOR3( 0.0f, 1.0f, 0.0f );

    D3DXVec3TransformCoord( &rotaxis, &axis0, &selectmat );

    D3DXVec3Normalize( &rotaxis, &rotaxis );

    rotrad = delta / 10.0f * (float)PAI / 12.0f;

  }else if( axiskind == PICK_Z ){

    axis0 = D3DXVECTOR3( 0.0f, 0.0f, 1.0f );

    D3DXVec3TransformCoord( &rotaxis, &axis0, &selectmat );

    D3DXVec3Normalize( &rotaxis, &rotaxis );

    rotrad = delta / 400.0f * (float)PAI / 12.0f;

  }else{

    _ASSERT( 0 );

    return 1;

  }

  D3DXMatrixTranslation( &befrotmat, -rootpos.x, -rootpos.y, -rootpos.z );

  D3DXMatrixTranslation( &aftrotmat, rootpos.x, rootpos.y, rootpos.z );

  D3DXMatrixRotationAxis( &rotmat, &rotaxis, rotrad );

  mat = befrotmat * rotmat * aftrotmat;

  D3DXVec3TransformCoord( &targetpos, &firstpos, &mat );



  CBone* curbone = firstbone;

  while( curbone && ((maxlevel == 0) || (levelcnt < maxlevel)) && 

    ( !curbone->m_faceptr || (curbone->m_faceptr && curbone->m_faceptr->m_bonetype == MIKOBONE_NORMAL) ) )

  {

    if( curbone->m_vertpos[BT_CHILD] != curbone->m_vertpos[BT_PARENT] ){

      D3DXVECTOR3 parworld, chilworld;

      chilworld = firstbone->m_childworld;

      D3DXVec3TransformCoord( &parworld, &(curbone->m_vertpos[BT_PARENT]), &(curbone->m_curmp.m_worldmat) );



      D3DXVECTOR3 chilaxis, taraxis;

      CalcShadowToPlane( chilworld, rotaxis, parworld, &chilaxis );

      CalcShadowToPlane( targetpos, rotaxis, parworld, &taraxis );



      D3DXVECTOR3 parbef, chilbef, tarbef;

      CBone* parbone = curbone->m_parent;

      if( parbone ){

        D3DXMATRIX invmat;

        D3DXMatrixInverse( &invmat, NULL, &(parbone->m_curmp.m_worldmat) );

        D3DXVec3TransformCoord( &parbef, &parworld, &invmat );

        D3DXVec3TransformCoord( &chilbef, &chilaxis, &invmat );

        D3DXVec3TransformCoord( &tarbef, &taraxis, &invmat );

      }else{

        parbef = parworld;

        chilbef = chilaxis;

        tarbef = taraxis;

      }



      D3DXVECTOR3 vec0, vec1;

      vec0 = chilbef - parbef;

      D3DXVec3Normalize( &vec0, &vec0 );

      vec1 = tarbef - parbef;

      D3DXVec3Normalize( &vec1, &vec1 );



      D3DXVECTOR3 rotaxisF;

      D3DXVec3Cross( &rotaxisF, &vec0, &vec1 );

      D3DXVec3Normalize( &rotaxisF, &rotaxisF );



      float rotdot, rotrad;

      rotdot = DXVec3Dot( &vec0, &vec1 );

      rotdot = min( 1.0f, rotdot );

      rotdot = max( -1.0f, rotdot );

      rotrad = (float)acos( rotdot );

      if( rotrad > 1.0e-4 ){

        CQuaternion rotq;

        rotq.SetAxisAndRot( rotaxisF, rotrad );

        CMotionPoint* newmp = 0;

        int existflag = 0;

        newmp = curbone->AddMotionPoint( m_curmotinfo->motid, m_curmotinfo->curframe, 1, &existflag );

        CQuaternion q0 = newmp->m_q;

        D3DXVECTOR3 eul0 = newmp->m_eul;



        if( existflag == 0 ){

          NEWMPELEM newmpe;

          newmpe.boneptr = curbone;

          newmpe.mpptr = newmp;

          m_newmplist.push_back( newmpe );

        }





        D3DXVECTOR3 firstdiff = firstbone->m_childworld - targetpos;

        float firstdist = DXVec3Length( &firstdiff );



        curbone->MultQ( m_curmotinfo->motid, m_curmotinfo->curframe, rotq );

        UpdateMatrix( curbone->m_boneno, &m_matWorld, &m_matVP );



        D3DXVECTOR3 aftdiff = firstbone->m_childworld - targetpos;

        float aftdist = DXVec3Length( &aftdiff );

        if( (axiskind == PICK_Z) && (aftdist >= firstdist) ){

          newmp->m_q = q0;

          newmp->m_eul = eul0;

          UpdateMatrix( curbone->m_boneno, &m_matWorld, &m_matVP );				

        }

      }

    }

    curbone = curbone->m_parent;

    levelcnt++;

  }



  return 0;

}

CCD方式を少し変更しています。
目標座標やボーンの位置を、親ボーンを通り指定軸を法線とする平面上に射影してから
それを親の座標系に戻して計算しいます。

こうすることにより回転軸が常に指定軸になります。

０７−０６、移動のIK

移動のIKはCModel::IKMoveとIKMoveAxisDeltaで行います。
IKMoveAxisDeltaも目標座標を決めた後にIKMoveを呼び出すだけなので
IKMoveだけを説明します。

int CModel::IKMove( int srcboneno, D3DXVECTOR3 targetworld )

{

  m_newmplist.erase( m_newmplist.begin(), m_newmplist.end() );



  CBone* curbone = m_bonelist[ srcboneno ];

  if( !curbone ){

    _ASSERT( 0 );

    return 1;

  }

  D3DXVECTOR3 orgobj;

  orgobj = curbone->m_vertpos[BT_CHILD];



  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  //!!!! この関数では、ボーン変形を考慮した、ローカル座標で計算を行う。

  //!!!! matWorldが初期状態でない場合は、targetを、invmatworldで、逆変換してから渡す。

  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!



  D3DXMATRIX invworld;

  D3DXMatrixInverse( &invworld, NULL, &m_matWorld );

  D3DXVECTOR3 target;

  D3DXVec3TransformCoord( &target, &targetworld, &invworld );





  D3DXMATRIX raw1, par1;

  raw1 = curbone->m_curmp.m_mat;



  CBone* parbone = curbone->m_parent;

  if( parbone ){

    par1 = parbone->m_curmp.m_totalmat;

  }else{

    D3DXMatrixIdentity( &par1 );

  }



///////

  // org * raw1 * mvm * par1 = target



  float alpha, beta, ganma;

  alpha = target.x;

  beta = target.y;

  ganma = target.z;



////////

  float a11, a12, a13, a21, a22, a23, a31, a32, a33, a41, a42, a43;

  a11 = raw1._11;

  a12 = raw1._12;

  a13 = raw1._13;



  a21 = raw1._21;

  a22 = raw1._22;

  a23 = raw1._23;



  a31 = raw1._31;

  a32 = raw1._32;

  a33 = raw1._33;



  a41 = raw1._41;

  a42 = raw1._42;

  a43 = raw1._43;



  float b11, b12, b13, b21, b22, b23, b31, b32, b33, b41, b42, b43;

  b11 = par1._11;

  b12 = par1._12;

  b13 = par1._13;



  b21 = par1._21;

  b22 = par1._22;

  b23 = par1._23;



  b31 = par1._31;

  b32 = par1._32;

  b33 = par1._33;



  b41 = par1._41;

  b42 = par1._42;

  b43 = par1._43;



  float c11, c12, c13, c21, c22, c23, c31, c32, c33, c41, c42, c43;

  c11 = a11*b11 + a12*b21 + a13*b31;

  c12 = a11*b12 + a12*b22 + a13*b32;

  c13 = a11*b13 + a12*b23 + a13*b33;



  c21 = a21*b11 + a22*b21 + a23*b31;

  c22 = a21*b12 + a22*b22 + a23*b32;

  c23 = a21*b13 + a22*b23 + a23*b33;



  c31 = a31*b11 + a32*b21 + a33*b31;

  c32 = a31*b12 + a32*b22 + a33*b32;

  c33 = a31*b13 + a32*b23 + a33*b33;



  c41 = b11*a41 + b21*a42 + b31*a43 + b41;

  c42 = b12*a41 + b22*a42 + b32*a43 + b42;

  c43 = b13*a41 + b23*a42 + b33*a43 + b43;



  //zero div check

  if( b33 == 0.0f )

    return 0;

  float B33 = 1.0f / b33;



  float p, q, r, s, t, v, w;

  float x, y, z;



  x = orgobj.x;

  y = orgobj.y;

  z = orgobj.z;



  //zero div check

  if( (b11 - b31 * B33 * b13) == 0.0f )

    return 0;



  p = B33 * ( ganma - c13 * x - c23 * y - c33 * z - c43 );

  q = 1.0f / (b11 - b31 * B33 * b13);

  r = q * (alpha - c11 * x - c21 * y - c31 * z - c41 - b31 * p);



  s = b12 - b32 * B33 * b13;

  t = beta - c12 * x - c22 * y - c32 * z - c42 - b32 * p;



  v = b21 - b31 * B33 * b23;

  w = b22 - b32 * B33 * b23;



  if( (w - s * q * v) == 0.0f ){

    return 0;

  }



  float mvx, mvy, mvz;

  mvy = ( t -  s * r ) / ( w - s * q * v );

  mvx = -q * ( b21 - b31 * B33 * b23 ) * mvy + r;

  mvz = p - B33 * b13 * mvx - B33 * b23 * mvy;



  CMotionPoint* newmp = 0;

  int existflag = 0;

  newmp = curbone->AddMotionPoint( m_curmotinfo->motid, m_curmotinfo->curframe, 1, &existflag );

  if( existflag == 0 ){

    NEWMPELEM newmpe;

    newmpe.boneptr = curbone;

    newmpe.mpptr = newmp;

    m_newmplist.push_back( newmpe );

  }



  curbone->AddTra( m_curmotinfo->motid, m_curmotinfo->curframe, mvx, mvy, mvz );

  UpdateMatrix( curbone->m_boneno, &m_matWorld, &m_matVP );

	

  return 0;

}

この計算はローカル座標で行うためまず目標位置にワールド行列の逆行列をかけてtargetを求めます。
あとは実に力技ですｗｗ。
ボーンのローカル座標をorg、ボーンのローカルボーン変形行列をraw1、
IKによる位置移動行列をmvm、親のボーンの行列をpar1、ローカルの目標位置をtargetとすると
以下の式が成り立ちます。

org * raw1 * mvm * par1 = target

この式を各成分について展開して連立方程式を解き(紙とえんぴつで)、導いた式で移動成分を求めます。
この移動をボーンの姿勢に足して出来上がりです。


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

トップページに戻る