/// <summary>
/// 矩阵转四元数
/// </summary>
/// <param name="q">输出</param>
/// <param name="m">输入矩阵</param>
/// <returns></returns>
public static FixedQuaternion MatrixQuat(this FixedMatrix4x4 m)
{
FixedQuaternion q = new FixedQuaternion();
Fixed s;
Fixed F25 = new Fixed(25, 100);
if (m.m00 > m.m11 && m.m00 > m.m22)
{
s = 2 * FixedMathf.Sqrt(1f + m.m00 - m.m11 - m.m22);
q[0] = F25 * s;
q[1] = (m.m10 + m.m01) / s;
q[2] = (m.m02 + m.m20) / s;
q[3] = (m.m21 - m.m12) / s;
}
else if (m.m11 > m.m22)
{
s = 2 * FixedMathf.Sqrt(1f + m.m11 - m.m00 - m.m22);
q[0] = (m.m10 + m.m01) / s;
q[1] = F25 * s;
q[2] = (m.m21 + m.m12) / s;
q[3] = (m.m02 - m.m20) / s;
}
else
{
s = 2 * FixedMathf.Sqrt(1f + m.m22 - m.m00 - m.m11);
q[0] = (m.m02 + m.m20) / s;
q[1] = (m.m21 + m.m12) / s;
q[2] = F25 * s;
q[3] = (m.m10 - m.m01) / s;
}
return(q);
}
/// <summary>
/// 四元数转矩阵
/// </summary>
/// <param name="m">输出矩阵</param>
/// <param name="q">输入四元数</param>
/// <returns></returns>
public static FixedMatrix4x4 QuatMatrix(this FixedMatrix4x4 m, FixedQuaternion q)
{
Fixed xx, xy, xz, xw;
Fixed yy, yz, yw;
Fixed zz, zw;
xx = q[0] * q[0];
xy = q[0] * q[1];
xz = q[0] * q[2];
xw = q[0] * q[3];
yy = q[1] * q[1];
yz = q[1] * q[2];
yw = q[1] * q[3];
zz = q[2] * q[2];
zw = q[2] * q[3];
m.m00 = 1 - 2 * (yy + zz);
m.m01 = 2 * (xy - zw);
m.m02 = 2 * (xz + yw);
m.m03 = 0;
m.m10 = 2 * (xy + zw);
m.m11 = 1 - 2 * (xx + zz);
m.m12 = 2 * (yz - xw);
m.m13 = 0;
m.m20 = 2 * (xz - yw);
m.m21 = 2 * (yz + xw);
m.m22 = 1 - 2 * (xx + yy);
m.m23 = 0;
m.m30 = 0;
m.m31 = 0;
m.m32 = 0;
m.m33 = 1;
return(m);
}
public static FixedQuaternion Lerp(FixedQuaternion a, FixedQuaternion b, Fixed f)
{
Fixed tempF = f;
Fixed temp1 = Fixed.One - tempF;
FixedQuaternion result = new FixedQuaternion();
Fixed temp2 = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
if (temp2 >= Fixed.Zero)
{
result.x = temp1 * a.x + tempF * b.x;
result.y = temp1 * a.y + tempF * b.y;
result.z = temp1 * a.z + tempF * b.z;
result.w = temp1 * a.w + tempF * b.w;
}
else
{
result.x = temp1 * a.x - tempF * b.x;
result.y = temp1 * a.y - tempF * b.y;
result.z = temp1 * a.z - tempF * b.z;
result.w = temp1 * a.w - tempF * b.w;
}
Fixed temp3 = Fixed.One / result.magnitude;
result.x *= temp3;
result.y *= temp3;
result.z *= temp3;
result.w *= temp3;
return(result);
}
public static FixedQuaternion LookRotation(FixedVector3 forward, FixedVector3 upwards)
{
forward = forward.normalized;
FixedVector3 right = FixedVector3.Cross(upwards, forward).normalized;
upwards = FixedVector3.Cross(forward, right);
var rightX = right.x;
var rightY = right.y;
var rightZ = right.z;
var upwardsX = upwards.x;
var upwardsY = upwards.y;
var upwardsZ = upwards.z;
var forwardX = forward.x;
var forwardY = forward.y;
var forwardZ = forward.z;
Fixed temp1 = rightX + upwardsY + forwardZ;
var quaternion = new FixedQuaternion();
if (temp1 > Fixed.Zero)
{
var num = FixedMathf.Sqrt(temp1 + Fixed.One);
quaternion.w = num * Fixed.Half;
num = Fixed.Half / num;
quaternion.x = (upwardsZ - forwardY) * num;
quaternion.y = (forwardX - rightZ) * num;
quaternion.z = (rightY - upwardsX) * num;
return(quaternion);
}
if (rightX >= upwardsY && rightX >= forwardZ)
{
var temp2 = FixedMathf.Sqrt(Fixed.One + rightX - upwardsY - forwardZ);
var temp3 = Fixed.Half / temp2;
quaternion.x = Fixed.Half * temp2;
quaternion.y = (rightY + upwardsX) * temp3;
quaternion.z = (rightZ + forwardX) * temp3;
quaternion.w = (upwardsZ - forwardY) * temp3;
return(quaternion);
}
if (upwardsY > forwardZ)
{
var temp4 = FixedMathf.Sqrt(Fixed.One + upwardsY - rightX - forwardZ);
var temp5 = Fixed.Half / temp4;
quaternion.x = (upwardsX + rightY) * temp5;
quaternion.y = Fixed.Half * temp4;
quaternion.z = (forwardY + upwardsZ) * temp5;
quaternion.w = (forwardX - rightZ) * temp5;
return(quaternion);
}
var temp6 = FixedMathf.Sqrt(Fixed.One + forwardZ - rightX - upwardsY);
var temp7 = Fixed.Half / temp6;
quaternion.x = (forwardX + rightZ) * temp7;
quaternion.y = (forwardY + upwardsZ) * temp7;
quaternion.z = Fixed.Half * temp6;
quaternion.w = (rightY - upwardsX) * temp7;
return(quaternion);
}
public FixedMatrix4x4(FixedQuaternion column0, FixedQuaternion column1, FixedQuaternion column2, FixedQuaternion column3)
{
this.m00 = column0.x;
this.m01 = column1.x;
this.m02 = column2.x;
this.m03 = column3.x;
this.m10 = column0.y;
this.m11 = column1.y;
this.m12 = column2.y;
this.m13 = column3.y;
this.m20 = column0.z;
this.m21 = column1.z;
this.m22 = column2.z;
this.m23 = column3.z;
this.m30 = column0.w;
this.m31 = column1.w;
this.m32 = column2.w;
this.m33 = column3.w;
}
public bool Equals(FixedQuaternion other)
{
return(x == other.x && y == other.y && z == other.z && w == other.w);
}
public static Fixed Angle(FixedQuaternion a, FixedQuaternion b)
{
Fixed f = Dot(a, b);
return(FixedMathf.Acos(FixedMathf.Min(FixedMathf.Abs(f), Fixed.One).AsFloat()) * new Fixed(2) * Fixed.Rad2Deg);
}
public static FixedQuaternion Slerp(FixedQuaternion a, FixedQuaternion b, Fixed f)
{
if (a.sqrMagnitude == Fixed.Zero)
{
if (b.sqrMagnitude == Fixed.Zero)
{
return(Identity);
}
return(b);
}
if (b.sqrMagnitude == Fixed.Zero)
{
return(a);
}
if (f > Fixed.One)
{
f = Fixed.One;
}
if (f < Fixed.Zero)
{
f = Fixed.Zero;
}
Fixed cosHalfAngle = a.w * b.w + FixedVector3.Dot(new FixedVector3(a.x, a.y, a.z), new FixedVector3(b.x, b.y, b.z));
if (cosHalfAngle >= Fixed.One || cosHalfAngle <= -Fixed.One)
{
return(a);
}
if (cosHalfAngle < Fixed.Zero)
{
b.x = -b.x;
b.y = -b.y;
b.z = -b.z;
b.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
Fixed blendA;
Fixed blendB;
if (cosHalfAngle < 0.999999)
{
Fixed halfAngle = FixedMathf.Acos(cosHalfAngle);
Fixed sinHalfAngle = FixedMathf.Sin(halfAngle);
Fixed oneOverSinHalfAngle = Fixed.One / sinHalfAngle;
blendA = FixedMathf.Sin(halfAngle * (Fixed.One - f)) * oneOverSinHalfAngle;
blendB = FixedMathf.Sin(halfAngle * f) * oneOverSinHalfAngle;
}
else
{
blendA = Fixed.One - f;
blendB = f;
}
FixedQuaternion result =
new FixedQuaternion(blendA * new FixedVector3(a.x, a.y, a.z) + blendB * new FixedVector3(b.x, b.y, b.z),
blendA * a.w + blendB * b.w);
if (result.sqrMagnitude > Fixed.Zero)
{
return(Normalize(result));
}
return(Identity);
}
public static Fixed Dot(FixedQuaternion a, FixedQuaternion b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
public static FixedQuaternion Normalize(FixedQuaternion c)
{
Fixed scale = Fixed.One / c.magnitude;
return(new FixedQuaternion(c.x * scale, c.y * scale, c.z * scale, c.w * scale));
}
