public PmdBone(PMD_Bone pPMDBoneData, PmdBone[] pBoneArray)
{
// ボーン名のコピー
this._name = pPMDBoneData.szName;
// 位置のコピー
this._pmd_bone_position.setValue(pPMDBoneData.vec3Position);
// 親ボーンの設定
if (pPMDBoneData.nParentNo != -1)
{
this._parent_bone = pBoneArray[pPMDBoneData.nParentNo];
m_vec3Offset.Vector3Sub(this._pmd_bone_position, this._parent_bone._pmd_bone_position);
}
else
{
// 親なし
this._parent_bone = null;
this.m_vec3Offset.setValue(this._pmd_bone_position);
}
// 子ボーンの設定
if (pPMDBoneData.nChildNo != -1)
{
this.m_pChildBone = pBoneArray[pPMDBoneData.nChildNo];
}
this.m_matInvTransform.MatrixIdentity();
this.m_matInvTransform.m[3, 0] = -this._pmd_bone_position.x;
this.m_matInvTransform.m[3, 1] = -this._pmd_bone_position.y;
this.m_matInvTransform.m[3, 2] = -this._pmd_bone_position.z;
this.m_bIKLimitAngle = false;
// 各変数の初期値を設定
reset();
}
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;
}