/// <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)); }