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 class PmdIK {
private PmdBone m_pTargetBone; // IKターゲットボーン
private PmdBone m_pEffBone; // IK先端ボーン
private int m_unCount;
private double _fact;
private int m_nSortVal;
private PmdBone[] m_ppBoneList; // IKを構成するボーンの配列
private final MmdVector3[] _work_vector3 = MmdVector3.createArray(4);
private final MmdVector4 _work_vector4 = new MmdVector4();
public PmdIK(PMD_IK pPMDIKData, PmdBone[] i_ref_bone_array) {
// IKターゲットボーン
this.m_pTargetBone = i_ref_bone_array[pPMDIKData.nTargetNo];
// IK先端ボーン
this.m_pEffBone = i_ref_bone_array[pPMDIKData.nEffNo];
this.m_unCount = pPMDIKData.unCount;
this._fact = pPMDIKData.fFact * Math.PI;
this.m_nSortVal = pPMDIKData.punLinkNo[0];
// IKリンク配列の作成
final int number_of_ik_link = pPMDIKData.cbNumLink;
this.m_ppBoneList = new PmdBone[number_of_ik_link]; // 参照
for (int i = 0; i < number_of_ik_link; i++) {
this.m_ppBoneList[i] = i_ref_bone_array[pPMDIKData.punLinkNo[i]]; // ボーン番号は降順で格納されている
if (this.m_ppBoneList[i].getName().equals("左ひざ")
|| this.m_ppBoneList[i].getName().equals("右ひざ")) {
this.m_ppBoneList[i].setIKLimitAngle(true);
}
}
}
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 int getSortVal() {
return this.m_nSortVal;
}
private final MmdMatrix __update_matInvBone = new MmdMatrix();
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;
}
}
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;
}