private PmdBone m_pNeckBone; // 首のボーン private void getMotionPosRot(MotionData pMotionData, float fFrame, MmdVector3 pvec3Pos, MmdVector4 pvec4Rot) { int i; int ulNumKeyFrame = pMotionData.ulNumKeyFrames; // 最終フレームを過ぎていた場合 if (fFrame > pMotionData.pKeyFrames[ulNumKeyFrame - 1].fFrameNo) { fFrame = pMotionData.pKeyFrames[ulNumKeyFrame - 1].fFrameNo; } // 現在の時間がどのキー近辺にあるか for (i = 0; i < ulNumKeyFrame; i++) { if (fFrame <= pMotionData.pKeyFrames[i].fFrameNo) { break; } } // 前後のキーを設定 int lKey0, lKey1; lKey0 = i - 1; lKey1 = i; if (lKey0 <= 0) { lKey0 = 0; } if (i == ulNumKeyFrame) { lKey1 = ulNumKeyFrame - 1; } // 前後のキーの時間 float fTime0 = pMotionData.pKeyFrames[lKey0].fFrameNo; float fTime1 = pMotionData.pKeyFrames[lKey1].fFrameNo; // 前後のキーの間でどの位置にいるか float fLerpValue; if (lKey0 != lKey1) { fLerpValue = (fFrame - fTime0) / (fTime1 - fTime0); pvec3Pos.Vector3Lerp(pMotionData.pKeyFrames[lKey0].vec3Position, pMotionData.pKeyFrames[lKey1].vec3Position, fLerpValue); pvec4Rot.QuaternionSlerp(pMotionData.pKeyFrames[lKey0].vec4Rotate, pMotionData.pKeyFrames[lKey1].vec4Rotate, fLerpValue); pvec4Rot.QuaternionNormalize(pvec4Rot);//これほんとにいるの? } else { pvec3Pos.setValue(pMotionData.pKeyFrames[lKey0].vec3Position); pvec4Rot.setValue(pMotionData.pKeyFrames[lKey0].vec4Rotate); } }
public void update() { MmdVector3 vec3OrgTargetPos = this.__update_vec3OrgTargetPos; MmdMatrix matInvBone = this.__update_matInvBone; vec3OrgTargetPos.x = (float)m_pTargetBone.m_matLocal.m[3, 0]; vec3OrgTargetPos.y = (float)m_pTargetBone.m_matLocal.m[3, 1]; vec3OrgTargetPos.z = (float)m_pTargetBone.m_matLocal.m[3, 2]; MmdVector3 vec3EffPos = this._work_vector3[0]; MmdVector3 vec3TargetPos = this._work_vector3[1]; MmdVector3 vec3Diff = this._work_vector3[2]; MmdVector3 vec3RotAxis = this._work_vector3[3]; MmdVector4 vec4RotQuat = this._work_vector4; for (int i = this.m_ppBoneList.Length - 1; i >= 0; i--) { this.m_ppBoneList[i].updateMatrix(); } m_pEffBone.updateMatrix(); for (int it = 0; it < m_unCount; it++) { for (int cbLinkIdx = 0; cbLinkIdx < this.m_ppBoneList.Length; cbLinkIdx++) { // エフェクタの位置の取得 vec3EffPos.x = (float)m_pEffBone.m_matLocal.m[3, 0]; vec3EffPos.y = (float)m_pEffBone.m_matLocal.m[3, 1]; vec3EffPos.z = (float)m_pEffBone.m_matLocal.m[3, 2]; // ワールド座標系から注目ノードの局所(ローカル)座標系への変換 matInvBone.MatrixInverse(m_ppBoneList[cbLinkIdx].m_matLocal); // エフェクタ,到達目標のローカル位置 vec3EffPos.Vector3Transform(vec3EffPos, matInvBone); vec3TargetPos.Vector3Transform(vec3OrgTargetPos, matInvBone); // 十分近ければ終了 vec3Diff.Vector3Sub(vec3EffPos, vec3TargetPos); if (vec3Diff.Vector3DotProduct(vec3Diff) < 0.0000001f) { return; } // (1) 基準関節→エフェクタ位置への方向ベクトル vec3EffPos.Vector3Normalize(vec3EffPos); // (2) 基準関節→目標位置への方向ベクトル vec3TargetPos.Vector3Normalize(vec3TargetPos); // ベクトル (1) を (2) に一致させるための最短回転量(Axis-Angle) // // 回転角 double fRotAngle = Math.Acos(vec3EffPos.Vector3DotProduct(vec3TargetPos)); if (0.00000001 < Math.Abs(fRotAngle)) { if (fRotAngle < -m_fFact) { fRotAngle = -m_fFact; } else if (m_fFact < fRotAngle) { fRotAngle = m_fFact; } // 回転軸 vec3RotAxis.Vector3CrossProduct(vec3EffPos, vec3TargetPos); if (vec3RotAxis.Vector3DotProduct(vec3RotAxis) < 0.0000001) { continue; } vec3RotAxis.Vector3Normalize(vec3RotAxis); // 関節回転量の補正 vec4RotQuat.QuaternionCreateAxis(vec3RotAxis, fRotAngle); if (m_ppBoneList[cbLinkIdx].m_bIKLimitAngle) { limitAngle(vec4RotQuat, vec4RotQuat); } vec4RotQuat.QuaternionNormalize(vec4RotQuat); m_ppBoneList[cbLinkIdx].m_vec4Rotate.QuaternionMultiply(m_ppBoneList[cbLinkIdx].m_vec4Rotate, vec4RotQuat); m_ppBoneList[cbLinkIdx].m_vec4Rotate.QuaternionNormalize(m_ppBoneList[cbLinkIdx].m_vec4Rotate); for (int i = cbLinkIdx; i >= 0; i--) { m_ppBoneList[i].updateMatrix(); } m_pEffBone.updateMatrix(); } } } return; }