// 四元数の累乗
public Qtrn Pow(Qtrn q, float exponent)
{
// 単位四元数チェック 0除算を防ぐ
if (Mathf.Abs(q.w) > 0.999f)
{
Debug.Log("単位四元数ではない");
return(q);
}
// 半分の角度alpha(=theta/2)を求める
// q = [cos(θ/2) sin(θ/2)V]
var alpha = Mathf.Acos(q.w);
// 新しいalpha値
var newAlpha = alpha * exponent;
// 新しいw値
var result = new Qtrn();
result.w = Mathf.Cos(newAlpha);
// 新しいxyz値
var mult = Mathf.Sin(newAlpha) / Mathf.Sin(alpha);
result.x = q.x * mult;
result.y = q.y * mult;
result.z = q.z * mult;
return(result);
}
public static Qtrn identity()
{
var q = new Qtrn();
q.Identity();
return(q);
}
// 球面線形補間
public Qtrn Slerp(Qtrn q0, Qtrn q1, float t)
{
// 範囲外チェック
if (t <= 0f)
{
return(q0);
}
if (1f <= t)
{
return(q1);
}
// 内積からCosを取得
var cosOmega = DotProduct(q0, q1);
// 負の内積の場合-q1を用いる
var sign = cosOmega < 0f ? 1 : -1;
var q1w = q1.w * sign;
var q1x = q1.x * sign;
var q1y = q1.y * sign;
var q1z = q1.z * sign;
cosOmega *= sign;
// 単位四元数のチェック逆じゃない?
float k0, k1;
if (cosOmega > 0.999f)
{
// 非常に近い --- 線形補間を用いる(0除算を防ぐため)
k0 = 1f - t;
k1 = t;
}
else
{
// sinを算出する
// sin^2(omega) + cos^2(omega) = 1
var sinOmega = Mathf.Sqrt(1f - cosOmega * cosOmega);
// 角度算出
var omega = Mathf.Atan2(sinOmega, cosOmega);
var oneOverSinOmega = 1f / sinOmega;
k0 = Mathf.Sin((1f - t) * omega) * oneOverSinOmega;
k1 = Mathf.Sin(t * omega) * oneOverSinOmega;
}
// 補間
var result = new Qtrn();
result.w = k0 * q0.w + k1 * q1.w;
result.x = k0 * q0.x + k1 * q1.x;
result.y = k0 * q0.y + k1 * q1.y;
result.z = k0 * q0.z + k1 * q1.z;
return(result);
}
// 外積
public static Qtrn operator *(Qtrn a, Qtrn b)
{
var result = new Qtrn();
result.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
result.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
result.y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z;
result.z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x;
return(result);
}
// 四元数の共役を返す
public Qtrn Conjugate(Qtrn q)
{
var result = new Qtrn();
// ベクトル部のみ反転させる
result.w = q.w;
result.x = -q.x;
result.y = -q.y;
result.z = -q.z;
return(result);
}
public void FromObjectToInertialQuaternion(Qtrn q)
{
m11 = 1f - 2f * (q.y * q.y - q.z * q.z);
m12 = 2f * (q.x * q.y - q.w * q.z);
m13 = 2f * (q.x * q.z + q.w * q.y);
m21 = 2f * (q.x * q.y + q.w * q.z);
m22 = 1f - 2f * (q.x * q.x - q.z * q.z);
m23 = 2f * (q.y * q.z - q.w * q.x);
m31 = 2f * (q.x * q.z - q.w * q.y);
m32 = 2f * (q.y * q.z + q.w * q.x);
m33 = 1f - 2 * (q.x * q.x - q.y * q.y);
}
// 角変位を四元数形式で与え、回転を実行する行列をセットアップする
// 平行移動部分はリセットされる
public void FromQuaternion(Qtrn q)
{
// 共通して用いる副次式を最適化するために値を計算する
var ww = 2f * q.w;
var xx = 2f * q.x;
var yy = 2f * q.y;
var zz = 2f * q.z;
// 行列の要素を設定する
m11 = 1f - yy * q.y - zz * q.z;
m12 = xx * q.y + ww * q.z;
m13 = xx * q.z - ww * q.y;
m21 = yy * q.x - ww * q.z;
m22 = 1f - xx * q.x - zz * q.z;
m23 = yy * q.z + ww * q.x;
m31 = zz * q.x + ww * q.y;
m32 = zz * q.y - ww * q.x;
m33 = 1f - xx * q.x - yy * q.y;
tx = ty = tz = 0f;
}
// 慣性空間->オブジェクト空間の回転を実行
public void FromInertialToObjectQuaternion(Qtrn q)
{
// sin(pitch)を取り出す
var sp = -2.0f * (q.y * q.z + q.w * q.x);
// ジンバルロックチェック
if (Mathf.Abs(sp) > 0.999f)
{
// 真上か真下を向いている
pitch = MathUtil.kPiOver2 * sp;
// ヘディングを計算し、バンクを0に設定する
heading = Mathf.Atan2(-q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0f;
}
else
{
// ジンバルロックなし
pitch = Mathf.Asin(sp);
heading = Mathf.Atan2(q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = Mathf.Atan2(q.x * q.y - q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
// 内積
public float DotProduct(Qtrn a, Qtrn b)
{
return(a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
}
