void QuaternionSlerp( Vector4 *pvec4Out, const Vector4 *pvec4Src1, const Vector4 *pvec4Src2, float fLerpValue )
{
#if 0
// Slerp
float dot = pvec4Src1->x * pvec4Src2->x + pvec4Src1->y * pvec4Src2->y + pvec4Src1->z * pvec4Src2->z + pvec4Src1->w * pvec4Src2->w;
// 反転処理
Vector4 vec4CorrectTarget;
if(dot < 0.f)
{
vec4CorrectTarget.x = -pvec4Src2->x;
vec4CorrectTarget.y = -pvec4Src2->y;
vec4CorrectTarget.z = -pvec4Src2->z;
vec4CorrectTarget.w = -pvec4Src2->w;
dot = -dot;
}
else
{
vec4CorrectTarget = *pvec4Src2;
}
// 誤差対策
if(dot >= 1.f){ dot = 1.f; }
float radian = acosf( dot );
if( fabsf( radian ) < 0.0000000001f ){ *pvec4Out = vec4CorrectTarget; return; }
float inverseSin = 1.f / sinf( radian );
float t0 = sinf( (1.f - fLerpValue) * radian ) * inverseSin;
float t1 = sinf( fLerpValue * radian ) * inverseSin;
pvec4Out->x = pvec4Src1->x * t0 + vec4CorrectTarget.x * t1;
pvec4Out->y = pvec4Src1->y * t0 + vec4CorrectTarget.y * t1;
pvec4Out->z = pvec4Src1->z * t0 + vec4CorrectTarget.z * t1;
pvec4Out->w = pvec4Src1->w * t0 + vec4CorrectTarget.w * t1;
#else
// Qlerp
float qr = pvec4Src1->x * pvec4Src2->x + pvec4Src1->y * pvec4Src2->y + pvec4Src1->z * pvec4Src2->z + pvec4Src1->w * pvec4Src2->w;
float t0 = 1.0f - fLerpValue;
if( qr < 0 )
{
pvec4Out->x = pvec4Src1->x * t0 - pvec4Src2->x * fLerpValue;
pvec4Out->y = pvec4Src1->y * t0 - pvec4Src2->y * fLerpValue;
pvec4Out->z = pvec4Src1->z * t0 - pvec4Src2->z * fLerpValue;
pvec4Out->w = pvec4Src1->w * t0 - pvec4Src2->w * fLerpValue;
}
else
{
pvec4Out->x = pvec4Src1->x * t0 + pvec4Src2->x * fLerpValue;
pvec4Out->y = pvec4Src1->y * t0 + pvec4Src2->y * fLerpValue;
pvec4Out->z = pvec4Src1->z * t0 + pvec4Src2->z * fLerpValue;
pvec4Out->w = pvec4Src1->w * t0 + pvec4Src2->w * fLerpValue;
}
QuaternionNormalize( pvec4Out, pvec4Out );
#endif
}
//----------------------------------------
// キーフレームを補間して位置と回転を返す
//----------------------------------------
void cMotionPlayer::getMotionPosRot( const MotionDataList *pMotionData, float fFrame, Vector3 *pvec3Pos, Vector4 *pvec4Rot )
{
unsigned long i;
unsigned long ulNumKeyFrame = pMotionData->ulNumKeyFrames;
BoneKeyFrameList *pKeyFrameTemp = pMotionData->pKeyFrameList;
// 最後まで進む
while( pKeyFrameTemp->pNext ) {
pKeyFrameTemp = pKeyFrameTemp->pNext;
}
// 最終フレームを過ぎていた場合
if( fFrame > pKeyFrameTemp->fFrameNo )
{
fFrame = pKeyFrameTemp->fFrameNo;
}
// 現在の時間がどのキー近辺にあるか
BoneKeyFrameList *pPrevKeyFrame;
pKeyFrameTemp = pMotionData->pKeyFrameList;
pPrevKeyFrame = pKeyFrameTemp;
for( i = 0 ; i < ulNumKeyFrame ; i++ )
{
if( fFrame <= pKeyFrameTemp->fFrameNo )
{
break;
}
pPrevKeyFrame = pKeyFrameTemp;
pKeyFrameTemp = pKeyFrameTemp->pNext;
}
// 前後のキーを設定
if( !pKeyFrameTemp ) pKeyFrameTemp = pPrevKeyFrame;
// 前後のキーの時間
float fTime0 = pPrevKeyFrame->fFrameNo;
float fTime1 = pKeyFrameTemp->fFrameNo;
// 前後のキーの間でどの位置にいるか
float fLerpValue;
if( pPrevKeyFrame != pKeyFrameTemp )
{
fLerpValue = (fFrame - fTime0) / (fTime1 - fTime0);
Vector3Lerp( pvec3Pos, &(pPrevKeyFrame->vec3Position), &(pKeyFrameTemp->vec3Position), fLerpValue );
QuaternionSlerp( pvec4Rot, &(pPrevKeyFrame->vec4Rotate), &(pKeyFrameTemp->vec4Rotate), fLerpValue );
QuaternionNormalize( pvec4Rot, pvec4Rot );
}
else
{
*pvec3Pos = pPrevKeyFrame->vec3Position;
*pvec4Rot = pPrevKeyFrame->vec4Rotate;
}
}
// -----------------------------------------------------------------------------
static void HandleAttitudeReset(void)
{
quat_[0] = -AccelerationVector()[Z_BODY_AXIS];
quat_[1] = -AccelerationVector()[Y_BODY_AXIS];
quat_[2] = AccelerationVector()[X_BODY_AXIS];
quat_[3] = 0.0;
quat_[0] += QuaternionNorm(quat_);
QuaternionNormalize(quat_);
reset_attitude_ = 0;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
PCONFIG_DATA config;
const real_T *in_xdot;
const real_T *in_x0;
InputRealPtrsType pin_params;
const boolean_T *in_reset;
real_T *out_x;
real_T *out_t;
real_T *xd;
real_T *xd_temp;
real_T *xd_temp2;
time_T initial_time;
time_T final_time;
quaternion phi;
quaternion q;
vector omegadot_temp;
const real_T *pomegadot;
int_T i;
/* Retrieve C object from the pointers vector */
config = ssGetPWork(S)[0];
xd = (real_T*) ssGetDWork(S,0);
xd_temp = (real_T*) ssGetDWork(S,1);
if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
out_x = ssGetOutputPortRealSignal(S, 1);
if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);
switch( intval(mxGetScalar(paramSpecificationsSource)) )
{
case 1:
initial_time = config->initial_time;
final_time = ssGetTaskTime(S,0);
break;
case 2:
initial_time = mxGetScalar(paramInitialTime);
final_time = mxGetScalar(paramFinalTime);
break;
case 3:
initial_time = *(pin_params[0]);
final_time = *(pin_params[1]);
break;
default:
ssSetErrorStatus(S,"Wrong integration algorithm selected");
return;
}
/* ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */
/* Reset the states */
if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
{
ssSetIWorkValue(S, 0, 0);
if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
{
/* Internal initial conditions */
for( i=0; i<config->nstates; i++ )
{
xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
}
}
else
{
/* External initial conditions */
memcpy(xd, in_x0, config->nstates*sizeof(real_T));
}
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = initial_time;
}
if( final_time > initial_time )
{
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
{
/* Euler algorithm */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
QuaternionProd(&xd[i], phi, &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd[i] += in_xdot[i] * (final_time-initial_time);
i++;
}
}
}
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
{
/* Runge-Kutta algorithm */
/* f1 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
QuaternionProd(&xd_temp[i], &xd_temp[i], q);
QuaternionProd(&xd_temp[i], q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] = in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f2 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f3 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(phi, phi, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = final_time;
/* f4 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd_temp[i] += in_xdot[i];
xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
i++;
}
}
}
}
else
{
if( !ssCallSystemWithTid(S,0,tid) ) return;
}
config->initial_time = final_time;
/* Update the outputs */
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = final_time;
}
//======
// 更新
//======
void cPMDIK::update( void )
{
Vector3 vec3OrgTargetPos;
vec3OrgTargetPos.x = m_pTargetBone->m_matLocal[3][0];
vec3OrgTargetPos.y = m_pTargetBone->m_matLocal[3][1];
vec3OrgTargetPos.z = m_pTargetBone->m_matLocal[3][2];
Vector3 vec3EffPos;
Vector3 vec3TargetPos;
for( short i = m_cbNumLink - 1 ; i >= 0 ; i-- ){ m_ppBoneList[i]->updateMatrix(); }
m_pEffBone->updateMatrix();
for( unsigned short it = 0 ; it < m_unCount ; it++ )
{
for( unsigned char cbLinkIdx = 0 ; cbLinkIdx < m_cbNumLink ; cbLinkIdx++ )
{
// エフェクタの位置の取得
vec3EffPos.x = m_pEffBone->m_matLocal[3][0];
vec3EffPos.y = m_pEffBone->m_matLocal[3][1];
vec3EffPos.z = m_pEffBone->m_matLocal[3][2];
// ワールド座標系から注目ノードの局所(ローカル)座標系への変換
Matrix matInvBone;
MatrixInverse( matInvBone, m_ppBoneList[cbLinkIdx]->m_matLocal );
// エフェクタ，到達目標のローカル位置
Vector3Transform( &vec3EffPos, &vec3EffPos, matInvBone );
Vector3Transform( &vec3TargetPos, &vec3OrgTargetPos, matInvBone );
// 十分近ければ終了
Vector3 vec3Diff;
Vector3Sub( &vec3Diff, &vec3EffPos, &vec3TargetPos );
if( Vector3DotProduct( &vec3Diff, &vec3Diff ) < 0.0000001f ) return;
// (1) 基準関節→エフェクタ位置への方向ベクトル
Vector3Normalize( &vec3EffPos, &vec3EffPos );
// (2) 基準関節→目標位置への方向ベクトル
Vector3Normalize( &vec3TargetPos, &vec3TargetPos );
// ベクトル (1) を (2) に一致させるための最短回転量（Axis-Angle）
//
// 回転角
float fRotAngle = acosf( Vector3DotProduct( &vec3EffPos, &vec3TargetPos ) );
if( 0.00000001f < fabsf( fRotAngle ) )
{
if( fRotAngle < -m_fFact ) fRotAngle = -m_fFact;
else if( m_fFact < fRotAngle ) fRotAngle = m_fFact;
// 回転軸
Vector3 vec3RotAxis;
Vector3CrossProduct( &vec3RotAxis, &vec3EffPos, &vec3TargetPos );
if( Vector3DotProduct( &vec3RotAxis, &vec3RotAxis ) < 0.0000001f ) continue;
Vector3Normalize( &vec3RotAxis, &vec3RotAxis );
// 関節回転量の補正
Vector4 vec4RotQuat;
QuaternionCreateAxis( &vec4RotQuat, &vec3RotAxis, fRotAngle );
if( m_ppBoneList[cbLinkIdx]->m_bIKLimitAngle ) limitAngle( &vec4RotQuat, &vec4RotQuat );
QuaternionNormalize( &vec4RotQuat, &vec4RotQuat );
QuaternionMultiply( &m_ppBoneList[cbLinkIdx]->m_vec4Rotation, &m_ppBoneList[cbLinkIdx]->m_vec4Rotation, &vec4RotQuat );
QuaternionNormalize( &m_ppBoneList[cbLinkIdx]->m_vec4Rotation, &m_ppBoneList[cbLinkIdx]->m_vec4Rotation );
for( short i = cbLinkIdx ; i >= 0 ; i-- ){ m_ppBoneList[i]->updateMatrix(); }
m_pEffBone->updateMatrix();
}
}
}
}
//==========================
// モーションデータの初期化
//==========================
bool cVMDMotion::initialize( unsigned char *pData )
{
release();
// ヘッダのチェック
VMD_Header *pVMDHeader = (VMD_Header *)pData;
if( strncmp( pVMDHeader->szHeader, "Vocaloid Motion Data 0002", 30 ) != 0 )
return false; // ファイル形式が違う
pData += sizeof( VMD_Header );
//-----------------------------------------------------
// ボーンのキーフレーム数を取得
unsigned long ulNumBoneKeyFrames = *((unsigned long *)pData);
pData += sizeof( unsigned long );
// まずはモーションデータ中のボーンごとのキーフレーム数をカウント
VMD_Motion *pVMDMotion = (VMD_Motion *)pData;
MotionDataList *pMotTemp;
m_ulNumMotionNodes = 0;
m_fMaxFrame = 0.0f;
for( unsigned long i = 0 ; i < ulNumBoneKeyFrames ; i++, pVMDMotion++ )
{
if( m_fMaxFrame < (float)pVMDMotion->ulFrameNo ) m_fMaxFrame = (float)pVMDMotion->ulFrameNo; // 最大フレーム更新
pMotTemp = m_pMotionDataList;
while( pMotTemp )
{
if( strncmp( pMotTemp->szBoneName, pVMDMotion->szBoneName, 15 ) == 0 ) break;
pMotTemp = pMotTemp->pNext;
}
if( !pMotTemp )
{
// リストにない場合は新規ノードを追加
MotionDataList *pNew = new MotionDataList;
strncpy( pNew->szBoneName, pVMDMotion->szBoneName, 15 );
pNew->szBoneName[15] = '\0';
pNew->ulNumKeyFrames = 0;
pNew->pKeyFrameList = NULL;
pNew->pNext = m_pMotionDataList;
m_pMotionDataList = pNew;
pMotTemp = pNew;
m_ulNumMotionNodes++;
}
BoneKeyFrameList *pKeyFrame = new BoneKeyFrameList;
pKeyFrame->fFrameNo = (float)pVMDMotion->ulFrameNo;
pKeyFrame->vec3Position = pVMDMotion->vec3Position;
pKeyFrame->pNext = NULL;
QuaternionNormalize( &pKeyFrame->vec4Rotate, &pVMDMotion->vec4Rotate );
if( !pMotTemp->pKeyFrameList )
{
pMotTemp->pKeyFrameList = pKeyFrame;
}
else
{
pKeyFrame->pNext = pMotTemp->pKeyFrameList;
pMotTemp->pKeyFrameList = pKeyFrame;
}
pMotTemp->ulNumKeyFrames++;
}
// キーフレーム配列を昇順にソート
pMotTemp = m_pMotionDataList;
while( pMotTemp )
{
// qsort( pMotTemp->pKeyFrameList, pMotTemp->ulNumKeyFrames, sizeof( BoneKeyFrame ), boneCompareFunc );
sortByFrame( pMotTemp );
pMotTemp = pMotTemp->pNext;
}
pData += sizeof( VMD_Motion ) * ulNumBoneKeyFrames;
//-----------------------------------------------------
// 表情のキーフレーム数を取得
unsigned long ulNumFaceKeyFrames = *((unsigned long *)pData);
pData += sizeof( unsigned long );
// モーションデータ中の表情ごとのキーフレーム数をカウント
VMD_Face *pVMDFace = (VMD_Face *)pData;
FaceDataList *pFaceTemp;
m_ulNumFaceNodes = 0;
for( unsigned long i = 0 ; i < ulNumFaceKeyFrames ; i++, pVMDFace++ )
{
if( m_fMaxFrame < (float)pVMDFace->ulFrameNo ) m_fMaxFrame = (float)pVMDFace->ulFrameNo; // 最大フレーム更新
pFaceTemp = m_pFaceDataList;
while( pFaceTemp )
{
if( strncmp( pFaceTemp->szFaceName, pVMDFace->szFaceName, 15 ) == 0 ) break;
pFaceTemp = pFaceTemp->pNext;
}
if( !pFaceTemp )
{
// リストにない場合は新規ノードを追加
FaceDataList *pNew = new FaceDataList;
strncpy( pNew->szFaceName, pVMDFace->szFaceName, 15 );
pNew->szFaceName[15] = '\0';
pNew->ulNumKeyFrames = 0;
pNew->pNext = m_pFaceDataList;
m_pFaceDataList = pNew;
pFaceTemp = pNew;
m_ulNumFaceNodes++;
}
FaceKeyFrameList *pKeyFrame = new FaceKeyFrameList;
pKeyFrame->fFrameNo = (float)pVMDFace->ulFrameNo;
pKeyFrame->fRate = pVMDFace->fFactor;
pKeyFrame->pNext = NULL;
if( !pFaceTemp->pKeyFrameList )
{
pFaceTemp->pKeyFrameList = pKeyFrame;
}
else
{
pKeyFrame->pNext = pFaceTemp->pKeyFrameList;
pFaceTemp->pKeyFrameList = pKeyFrame;
}
pFaceTemp->ulNumKeyFrames++;
}
// キーフレーム配列を昇順にソート
pFaceTemp = m_pFaceDataList;
while( pFaceTemp )
{
// qsort( pFaceTemp->pKeyFrameList, pFaceTemp->ulNumKeyFrames, sizeof( FaceKeyFrame ), faceCompareFunc );
sortByFrame( pFaceTemp );
pFaceTemp = pFaceTemp->pNext;
}
return true;
}
//==========================
// モーションデータの初期化
//==========================
bool cVMDMotion::initialize( unsigned char *pData )
{
// ヘッダのチェック
VMD_Header *pVMDHeader = (VMD_Header *)pData;
if( strncmp( pVMDHeader->szHeader, "Vocaloid Motion Data 0002", 30 ) != 0 )
return false; // ファイル形式が違う
// 2009/07/15 Ru--en 新データが読み込めそうならば過去のデータ解放
release();
pData += sizeof( VMD_Header );
//-----------------------------------------------------
// ボーンのキーフレーム数を取得
unsigned int ulNumBoneKeyFrames = *((unsigned int *)pData);
pData += sizeof( unsigned int );
// まずはモーションデータ中のボーンごとのキーフレーム数をカウント
VMD_Motion *pVMDMotion = (VMD_Motion *)pData;
MotionDataList *pMotTemp;
m_fMaxFrame = 0.0f;
for( unsigned int i = 0 ; i < ulNumBoneKeyFrames ; i++, pVMDMotion++ )
{
if( m_fMaxFrame < (float)pVMDMotion->ulFrameNo ) m_fMaxFrame = (float)pVMDMotion->ulFrameNo; // 最大フレーム更新
pMotTemp = m_pMotionDataList;
while( pMotTemp )
{
if( strncmp( pMotTemp->szBoneName, pVMDMotion->szBoneName, 15 ) == 0 )
{
// リストに追加済みのボーン
pMotTemp->ulNumKeyFrames++;
break;
}
pMotTemp = pMotTemp->pNext;
}
if( !pMotTemp )
{
// リストにない場合は新規ノードを追加
MotionDataList *pNew = new MotionDataList;
strncpy( pNew->szBoneName, pVMDMotion->szBoneName, 15 ); pNew->szBoneName[15] = '\0';
pNew->ulNumKeyFrames = 1;
pNew->pNext = m_pMotionDataList;
m_pMotionDataList = pNew;
}
}
// キーフレーム配列を確保
pMotTemp = m_pMotionDataList;
m_ulNumMotionNodes = 0;
while( pMotTemp )
{
pMotTemp->pKeyFrames = new BoneKeyFrame[pMotTemp->ulNumKeyFrames];
pMotTemp->ulNumKeyFrames = 0; // 配列インデックス用にいったん0にする
pMotTemp = pMotTemp->pNext;
m_ulNumMotionNodes++;
}
// ボーンごとにキーフレームを格納
pVMDMotion = (VMD_Motion *)pData;
for( unsigned int i = 0 ; i < ulNumBoneKeyFrames ; i++, pVMDMotion++ )
{
pMotTemp = m_pMotionDataList;
while( pMotTemp )
{
if( strncmp( pMotTemp->szBoneName, pVMDMotion->szBoneName, 15 ) == 0 )
{
BoneKeyFrame *pKeyFrame = &(pMotTemp->pKeyFrames[pMotTemp->ulNumKeyFrames]);
pKeyFrame->fFrameNo = (float)pVMDMotion->ulFrameNo;
pKeyFrame->vec3Position = pVMDMotion->vec3Position;
QuaternionNormalize( &pKeyFrame->vec4Rotation, &pVMDMotion->vec4Rotation );
pKeyFrame->clPosXInterBezier.initialize( pVMDMotion->cInterpolationX[0], pVMDMotion->cInterpolationX[4], pVMDMotion->cInterpolationX[8], pVMDMotion->cInterpolationX[12] );
pKeyFrame->clPosYInterBezier.initialize( pVMDMotion->cInterpolationY[0], pVMDMotion->cInterpolationY[4], pVMDMotion->cInterpolationY[8], pVMDMotion->cInterpolationY[12] );
pKeyFrame->clPosZInterBezier.initialize( pVMDMotion->cInterpolationZ[0], pVMDMotion->cInterpolationZ[4], pVMDMotion->cInterpolationZ[8], pVMDMotion->cInterpolationZ[12] );
pKeyFrame->clRotInterBezier.initialize( pVMDMotion->cInterpolationRot[0], pVMDMotion->cInterpolationRot[4], pVMDMotion->cInterpolationRot[8], pVMDMotion->cInterpolationRot[12] );
pMotTemp->ulNumKeyFrames++;
break;
}
pMotTemp = pMotTemp->pNext;
}
}
// キーフレーム配列を昇順にソート
pMotTemp = m_pMotionDataList;
while( pMotTemp )
{
qsort( pMotTemp->pKeyFrames, pMotTemp->ulNumKeyFrames, sizeof( BoneKeyFrame ), boneCompareFunc );
pMotTemp = pMotTemp->pNext;
}
pData += sizeof( VMD_Motion ) * ulNumBoneKeyFrames;
//-----------------------------------------------------
// 表情のキーフレーム数を取得
unsigned int ulNumFaceKeyFrames = *((unsigned int *)pData);
pData += sizeof( unsigned int );
// モーションデータ中の表情ごとのキーフレーム数をカウント
VMD_Face *pVMDFace = (VMD_Face *)pData;
FaceDataList *pFaceTemp;
for( unsigned int i = 0 ; i < ulNumFaceKeyFrames ; i++, pVMDFace++ )
{
if( m_fMaxFrame < (float)pVMDFace->ulFrameNo ) m_fMaxFrame = (float)pVMDFace->ulFrameNo; // 最大フレーム更新
pFaceTemp = m_pFaceDataList;
while( pFaceTemp )
{
if( strncmp( pFaceTemp->szFaceName, pVMDFace->szFaceName, 15 ) == 0 )
{
// リストに追加済み
pFaceTemp->ulNumKeyFrames++;
break;
}
pFaceTemp = pFaceTemp->pNext;
}
if( !pFaceTemp )
{
// リストにない場合は新規ノードを追加
FaceDataList *pNew = new FaceDataList;
strncpy( pNew->szFaceName, pVMDFace->szFaceName, 15 ); pNew->szFaceName[15] = '\0';
pNew->ulNumKeyFrames = 1;
pNew->pNext = m_pFaceDataList;
m_pFaceDataList = pNew;
}
}
// キーフレーム配列を確保
pFaceTemp = m_pFaceDataList;
m_ulNumFaceNodes = 0;
while( pFaceTemp )
{
pFaceTemp->pKeyFrames = new FaceKeyFrame[pFaceTemp->ulNumKeyFrames];
pFaceTemp->ulNumKeyFrames = 0; // 配列インデックス用にいったん0にする
pFaceTemp = pFaceTemp->pNext;
m_ulNumFaceNodes++;
}
// 表情ごとにキーフレームを格納
pVMDFace = (VMD_Face *)pData;
for( unsigned int i = 0 ; i < ulNumFaceKeyFrames ; i++, pVMDFace++ )
{
pFaceTemp = m_pFaceDataList;
while( pFaceTemp )
{
if( strncmp( pFaceTemp->szFaceName, pVMDFace->szFaceName, 15 ) == 0 )
{
FaceKeyFrame *pKeyFrame = &(pFaceTemp->pKeyFrames[pFaceTemp->ulNumKeyFrames]);
pKeyFrame->fFrameNo = (float)pVMDFace->ulFrameNo;
pKeyFrame->fRate = pVMDFace->fFactor;
pFaceTemp->ulNumKeyFrames++;
break;
}
pFaceTemp = pFaceTemp->pNext;
}
}
// キーフレーム配列を昇順にソート
pFaceTemp = m_pFaceDataList;
while( pFaceTemp )
{
qsort( pFaceTemp->pKeyFrames, pFaceTemp->ulNumKeyFrames, sizeof( FaceKeyFrame ), faceCompareFunc );
pFaceTemp = pFaceTemp->pNext;
}
return true;
}
void PmxBonePerformTransform(PMX_BONE* bone)
{
float rotation[4] = IDENTITY_QUATERNION;
float position[3] = {0};
if((bone->flags & PMX_BONE_FLAG_HAS_INHERENT_ROTATION) != 0)
{
PMX_BONE *parent_bone = bone->parent_inherent_bone;
if(parent_bone != NULL)
{
if((parent_bone->flags & PMX_BONE_FLAG_HAS_INHERENT_ROTATION) != 0)
{
MultiQuaternion(rotation, parent_bone->local_inherent_rotation);
}
else
{
float rotate_value[4];
COPY_VECTOR4(rotate_value, parent_bone->local_rotation);
MultiQuaternion(rotate_value, parent_bone->local_morph_rotation);
MultiQuaternion(rotation, rotate_value);
//MultiQuaternion(rotation, parent_bone->local_morph_rotation);
//MultiQuaternion(rotation, parent_bone->local_rotation);
}
}
if(FuzzyZero(bone->coefficient - 1.0f) == 0)
{
float set_value[4] = IDENTITY_QUATERNION;
QuaternionSlerp(set_value, set_value, rotation, bone->coefficient);
COPY_VECTOR4(rotation, set_value);
}
if(parent_bone != NULL && (parent_bone->flags & PMX_BONE_FLAG_HAS_INVERSE_KINEMATICS) != 0)
{
MultiQuaternion(rotation, parent_bone->joint_rotation);
}
COPY_VECTOR4(bone->local_inherent_rotation, rotation);
MultiQuaternion(bone->local_inherent_rotation, bone->local_rotation);
MultiQuaternion(bone->local_inherent_rotation, bone->local_morph_rotation);
QuaternionNormalize(bone->local_inherent_rotation);
}
MultiQuaternion(rotation, bone->local_rotation);
MultiQuaternion(rotation, bone->local_morph_rotation);
MultiQuaternion(rotation, bone->joint_rotation);
QuaternionNormalize(rotation);
if((bone->flags & PMX_BONE_FLAG_HAS_INHERENT_TRANSLATION) != 0)
{
PMX_BONE *parent_bone = bone->parent_inherent_bone;
if(parent_bone != NULL)
{
if((parent_bone->flags & PMX_BONE_FLAG_HAS_INHERENT_TRANSLATION) != 0)
{
position[0] += parent_bone->local_inherent_translation[0];
position[1] += parent_bone->local_inherent_translation[1];
position[2] += parent_bone->local_inherent_translation[2];
}
else
{
position[0] += parent_bone->local_translation[0] + parent_bone->local_morph_translation[0];
position[1] += parent_bone->local_translation[1] + parent_bone->local_morph_translation[1];
position[2] += parent_bone->local_translation[2] + parent_bone->local_morph_translation[2];
}
}
if(FuzzyZero(bone->coefficient - 1) == 0)
{
position[0] *= bone->coefficient;
position[1] *= bone->coefficient;
position[2] *= bone->coefficient;
}
COPY_VECTOR3(bone->local_inherent_translation, position);
}
position[0] += bone->local_translation[0] + bone->local_morph_translation[0];
position[1] += bone->local_translation[1] + bone->local_morph_translation[1];
position[2] += bone->local_translation[2] + bone->local_morph_translation[2];
PmxBoneUpdateWorldTransform(bone, position, rotation);
}
