private void limitAngle(MmdVector4 pvec4Out, MmdVector4 pvec4Src) {
final MmdVector3 vec3Angle = this._work_vector3[0];
// XYZ軸回転の取得
vec3Angle.QuaternionToEuler(pvec4Src);
// 角度制限
if (vec3Angle.x < -Math.PI) {
vec3Angle.x = (float) -Math.PI;
}
if (-0.002f < vec3Angle.x) {
vec3Angle.x = -0.002f;
}
vec3Angle.y = 0.0f;
vec3Angle.z = 0.0f;
// XYZ軸回転からクォータニオンへ
pvec4Out.QuaternionCreateEuler(vec3Angle);
return;
}
public void update() {
final MmdMatrix matInvBone = this.__update_matInvBone;
final MmdVector3 vec3EffPos = this._work_vector3[0];
final MmdVector3 vec3TargetPos = this._work_vector3[1];
final MmdVector3 vec3Diff = this._work_vector3[2];
final MmdVector3 vec3RotAxis = this._work_vector3[3];
final MmdVector4 vec4RotQuat = this._work_vector4;
// 事前に全Boneをupdateしてるなら、このコードは要らない?
for (int i = this.m_ppBoneList.length - 1; i >= 0; i--) {
this.m_ppBoneList[i].updateMatrix();
}
m_pEffBone.updateMatrix();
for (int it = this.m_unCount - 1; it >= 0; it--) {
for (int j = 0; j < this.m_ppBoneList.length; j++) {
// エフェクタの位置の取得
// ワールド座標系から注目ノードの局所(ローカル)座標系への変換
matInvBone.inverse(m_ppBoneList[j].m_matLocal);
// エフェクタ，到達目標のローカル位置
vec3EffPos.Vector3Transform(m_pEffBone.m_matLocal, matInvBone);
vec3TargetPos.Vector3Transform(m_pTargetBone.m_matLocal, 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 < -this._fact) {
fRotAngle = -this._fact;
} else if (this._fact < fRotAngle) {
fRotAngle = this._fact;
}
// 回転軸
vec3RotAxis.Vector3CrossProduct(vec3EffPos, vec3TargetPos);
if (vec3RotAxis.Vector3DotProduct(vec3RotAxis) < 0.0000001) {
continue;
}
vec3RotAxis.Vector3Normalize(vec3RotAxis);
// 関節回転量の補正
vec4RotQuat.QuaternionCreateAxis(vec3RotAxis, fRotAngle);
if (m_ppBoneList[j].m_bIKLimitAngle) {
limitAngle(vec4RotQuat, vec4RotQuat);
}
vec4RotQuat.QuaternionNormalize(vec4RotQuat);
m_ppBoneList[j].m_vec4Rotate.QuaternionMultiply(
m_ppBoneList[j].m_vec4Rotate, vec4RotQuat);
m_ppBoneList[j].m_vec4Rotate.QuaternionNormalize(m_ppBoneList[j].m_vec4Rotate);
for (int i = j; i >= 0; i--) {
m_ppBoneList[i].updateMatrix();
}
m_pEffBone.updateMatrix();
}
}
}
return;
}