public void QuaternionToMatrix(MmdVector4 pvec4Quat)
{
double x2 = pvec4Quat.x * pvec4Quat.x * 2.0f;
double y2 = pvec4Quat.y * pvec4Quat.y * 2.0f;
double z2 = pvec4Quat.z * pvec4Quat.z * 2.0f;
double xy = pvec4Quat.x * pvec4Quat.y * 2.0f;
double yz = pvec4Quat.y * pvec4Quat.z * 2.0f;
double zx = pvec4Quat.z * pvec4Quat.x * 2.0f;
double xw = pvec4Quat.x * pvec4Quat.w * 2.0f;
double yw = pvec4Quat.y * pvec4Quat.w * 2.0f;
double zw = pvec4Quat.z * pvec4Quat.w * 2.0f;
double[,] mt = this.m;
mt[0, 0] = 1.0f - y2 - z2;
mt[0, 1] = xy + zw;
mt[0, 2] = zx - yw;
mt[1, 0] = xy - zw;
mt[1, 1] = 1.0f - z2 - x2;
mt[1, 2] = yz + xw;
mt[2, 0] = zx + yw;
mt[2, 1] = yz - xw;
mt[2, 2] = 1.0f - x2 - y2;
mt[0, 3] = mt[1, 3] = mt[2, 3] = mt[3, 0] = mt[3, 1] = mt[3, 2] = 0.0f;
mt[3, 3] = 1.0f;
return;
}
public void setValue(MmdVector4 v)
{
this.x = v.x;
this.y = v.y;
this.z = v.z;
this.w = v.w;
return;
}
public void QuaternionNormalize(MmdVector4 pvec4Src)
{
double fSqr = 1.0 / Math.Sqrt((pvec4Src.x * pvec4Src.x + pvec4Src.y * pvec4Src.y + pvec4Src.z * pvec4Src.z + pvec4Src.w * pvec4Src.w));
this.x = (pvec4Src.x * fSqr);
this.y = (pvec4Src.y * fSqr);
this.z = (pvec4Src.z * fSqr);
this.w = (pvec4Src.w * fSqr);
}
public void QuaternionToEuler(MmdVector4 pvec4Quat)
{
// XYZ軸回転の取得
// Y回転を求める
double x2 = pvec4Quat.x + pvec4Quat.x;
double y2 = pvec4Quat.y + pvec4Quat.y;
double z2 = pvec4Quat.z + pvec4Quat.z;
double xz2 = pvec4Quat.x * z2;
double wy2 = pvec4Quat.w * y2;
double temp = -(xz2 - wy2);
// 誤差対策
if (temp >= 1.0)
{
temp = 1.0;
}
else if (temp <= -1.0)
{
temp = -1.0;
}
double yRadian = Math.Sin(temp);
// 他の回転を求める
double xx2 = pvec4Quat.x * x2;
double xy2 = pvec4Quat.x * y2;
double zz2 = pvec4Quat.z * z2;
double wz2 = pvec4Quat.w * z2;
if (yRadian < 3.1415926f * 0.5f)
{
if (yRadian > -3.1415926f * 0.5f)
{
double yz2 = pvec4Quat.y * z2;
double wx2 = pvec4Quat.w * x2;
double yy2 = pvec4Quat.y * y2;
this.x = (float)Math.Atan2(yz2 + wx2, (1.0f - (xx2 + yy2)));
this.y = (float)yRadian;
this.z = (float)Math.Atan2((xy2 + wz2), (1.0f - (yy2 + zz2)));
}
else
{
this.x = (float)-Math.Atan2((xy2 - wz2), (1.0f - (xx2 + zz2)));
this.y = (float)yRadian;
this.z = 0.0f;
}
}
else
{
this.x = (float)Math.Atan2((xy2 - wz2), (1.0f - (xx2 + zz2)));
this.y = (float)yRadian;
this.z = 0.0f;
}
}
public void QuaternionMultiply(MmdVector4 pvec4Src1, MmdVector4 pvec4Src2)
{
double px, py, pz, pw;
double qx, qy, qz, qw;
px = pvec4Src1.x; py = pvec4Src1.y; pz = pvec4Src1.z; pw = pvec4Src1.w;
qx = pvec4Src2.x; qy = pvec4Src2.y; qz = pvec4Src2.z; qw = pvec4Src2.w;
this.x = pw * qx + px * qw + py * qz - pz * qy;
this.y = pw * qy - px * qz + py * qw + pz * qx;
this.z = pw * qz + px * qy - py * qx + pz * qw;
this.w = pw * qw - px * qx - py * qy - pz * qz;
}
public void QuaternionSlerp(MmdVector4 pvec4Src1, MmdVector4 pvec4Src2, double fLerpValue)
{
// Qlerp
double qr = pvec4Src1.x * pvec4Src2.x + pvec4Src1.y * pvec4Src2.y + pvec4Src1.z * pvec4Src2.z + pvec4Src1.w * pvec4Src2.w;
double t0 = 1.0f - fLerpValue;
if (qr < 0)
{
this.x = pvec4Src1.x * t0 - pvec4Src2.x * fLerpValue;
this.y = pvec4Src1.y * t0 - pvec4Src2.y * fLerpValue;
this.z = pvec4Src1.z * t0 - pvec4Src2.z * fLerpValue;
this.w = pvec4Src1.w * t0 - pvec4Src2.w * fLerpValue;
}
else
{
this.x = pvec4Src1.x * t0 + pvec4Src2.x * fLerpValue;
this.y = pvec4Src1.y * t0 + pvec4Src2.y * fLerpValue;
this.z = pvec4Src1.z * t0 + pvec4Src2.z * fLerpValue;
this.w = pvec4Src1.w * t0 + pvec4Src2.w * fLerpValue;
}
QuaternionNormalize(this);
return;
}
