簡易マッスルボーン2

変換マトリクス作成関数とEvaluate関数のコード

//ローカル→ワールド変換
LW_CMatrix_4x4 LtoW(LWItemID id, const LWItemInfo *iinfo, double time)
{
  LWItemID    parent;
  LW_CMatrix_4x4  mat;
  LWDVector    vec;
 
  mat.init();

  iinfo->param(id, LWIP_ROTATION, time, vec);
  mat.rotateHPB(vec[0], vec[1], vec[2]);

  iinfo->param(id, LWIP_POSITION, time, vec);
  mat.translate(vec[0], vec[1], vec[2]);
 
  parent = iinfo->parent(id);
  if( parent ){
    mat = mat * LtoW(parent, iinfo, time);
  }
 
  return mat;
}

//ワールド→ローカル変換
LW_CMatrix_4x4 WtoL(LWItemID id, const LWItemInfo *iinfo, double time)
{
  LWItemID    parent;
  LW_CMatrix_4x4  mat;
  LWDVector    vec;
  int        i;
 
  mat.init();

  iinfo->param(id, LWIP_POSITION, time, vec);
  for(i=0; i<3; i++) vec[i] *= -1;
  mat.translate(vec[0], vec[1], vec[2]);
 
  iinfo->param(id, LWIP_ROTATION, time, vec);
  for(i=0; i<3; i++) vec[i] *= -1;
  mat.rotateY(vec[0]);
  mat.rotateX(vec[1]);
  mat.rotateZ(vec[2]);

  parent = iinfo->parent(id);
  if( parent ){
    mat = WtoL(parent, iinfo, time) * mat;
  }
 
  return mat;
}

XCALL_(void) Evaluate(LWInstance inst, const LWItemMotionAccess *access)
{
  MyData      *data = (MyData*)inst;
  LWDVector    rot, scl, tip;
  double      length;
  LW_CMatrix_4x4  mat;
 
  *data->self = access->item;
  if( NULL == *data->reference ) return;

  memcpy(tip, data->refPos, sizeof(double)*3);
  //ローカル(reference)→ワールド
  mat = LtoW(*data->reference, data->iinfo, access->time);
  mat.Transform(tip);
#ifdef CHECK_VAL
  if( fp )
    fprintf(fp, CHECK_FORMAT, access->time, tip[0], tip[1], tip[2], "ローカル→ワールド");
#endif//CHECK_VAL

  //ワールド→ローカル(self)
  mat = WtoL(*data->self, data->iinfo, access->time);
  mat.Transform(tip);
#ifdef CHECK_VAL
  if( fp )
    fprintf(fp, CHECK_FORMAT, access->time, tip[0], tip[1], tip[2], "ワールド→ローカル");
#endif//CHECK_VAL

  access->getParam(LWIP_ROTATION, access->time, rot);
  mat.init();
  mat.rotateHPB(rot[0], rot[1], rot[2]);
  mat.Transform(tip);
  Angle(tip, rot);
  length = Distance(tip);

  access->getParam(LWIP_SCALING, access->time, scl);
  scl[2] = length / data->restLength;
  if( data->holdVolume ){
    scl[0] = scl[1] = sqrt(1/scl[2]);
  }

  access->setParam(LWIP_ROTATION, rot);
  access->setParam(LWIP_SCALING, scl);
}

LW_CMatrix_4x4は自作の変換マトリクスクラス
LtoWとWtoL関数は再帰的に呼び出して、Root(親が無い)に達すると戻ってくる
Rootに向かって行くときに自分自身の変換マトリクスを作っておいて、帰るときに掛け合わせている
ワールド→ローカルとローカル→ワールドでは移動、回転のかける順番が違う