/// <summary>
/// 欧拉角转矩阵
/// </summary>
/// <param name="angles"></param>
/// <param name="m"></param>
/// <returns></returns>
public static FixedMatrix4x4 AngleMatrix(this FixedMatrix4x4 m, FixedVector3 angles)
{
Fixed cx, sx, cy, sy, cz, sz;
Fixed yx, yy;
cx = FixedMathf.Cos(angles.x * FixedMathf.Deg2Rad);
sx = FixedMathf.Sin(angles.x * FixedMathf.Deg2Rad);
cy = FixedMathf.Cos(angles.y * FixedMathf.Deg2Rad);
sy = FixedMathf.Sin(angles.y * FixedMathf.Deg2Rad);
cz = FixedMathf.Cos(angles.z * FixedMathf.Deg2Rad);
sz = FixedMathf.Sin(angles.z * FixedMathf.Deg2Rad);
yx = sy * cx;
yy = sy * sx;
m.m00 = cy * cz;
m.m01 = -cy * sz;
m.m02 = sy;
m.m03 = 0;
m.m10 = yy * cz + cx * sz;
m.m11 = -yy * sz + cx * cz;
m.m12 = -sx;
m.m13 = 0;
m.m20 = -yx * cz + sx * sz;
m.m21 = yx * sz + sx * cz;
m.m22 = cx * cy;
m.m23 = 0;
m.m30 = 0;
m.m31 = 0;
m.m32 = 0;
m.m33 = 1;
return(m);
}
public FixedQuaternion(FixedVector3 vector3, Fixed w)
{
x = vector3.x;
y = vector3.y;
z = vector3.z;
this.w = w;
}
/// <summary>
/// 矩阵转欧拉角
/// </summary>
/// <param name="m"></param>
/// <param name="angles">3个角度</param>
/// <returns></returns>
public static FixedVector3 MatrixAngle(this FixedMatrix4x4 m, FixedVector3 angles)
{
Fixed c;
Fixed tx, ty;
angles = new FixedVector3();
angles.y = FixedMathf.Asin(m.m02);
c = FixedMathf.Cos(angles.y);
if (FixedMathf.Abs(c) > 0.005f)
{
tx = m.m22 / c;
ty = -m.m12 / c;
angles.x = FixedMathf.Atan2(ty, tx);
tx = m.m00 / c;
ty = -m.m01 / c;
angles.z = FixedMathf.Atan2(ty, tx);
}
else
{
angles.x = 0;
tx = m.m11;
ty = m.m10;
angles.z = FixedMathf.Atan2(ty, tx);
}
return(angles);
}
public static FixedVector3 Cross(FixedVector3 lhs, FixedVector3 rhs)
{
return(new FixedVector3(
lhs.y * rhs.z - lhs.z * rhs.y,
lhs.z * rhs.x - lhs.x * rhs.z,
lhs.x * rhs.y - lhs.y * rhs.x));
}
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);
}
/// <summary>
/// <para>Transforms a direction by this matrix.</para>
/// </summary>
/// <param name="vector"></param>
public FixedVector3 MultiplyVector(FixedVector3 vector)
{
FixedVector3 vector3;
vector3.x = (Fixed)((Fixed)this.m00 * (Fixed)vector.x + (Fixed)this.m01 * (Fixed)vector.y + (Fixed)this.m02 * (Fixed)vector.z);
vector3.y = (Fixed)((Fixed)this.m10 * (Fixed)vector.x + (Fixed)this.m11 * (Fixed)vector.y + (Fixed)this.m12 * (Fixed)vector.z);
vector3.z = (Fixed)((Fixed)this.m20 * (Fixed)vector.x + (Fixed)this.m21 * (Fixed)vector.y + (Fixed)this.m22 * (Fixed)vector.z);
return(vector3);
}
/// <summary>
/// <para>Transforms a position by this matrix (fast).</para>
/// </summary>
/// <param name="point"></param>
public FixedVector3 MultiplyPoint3x4(FixedVector3 point)
{
FixedVector3 vector3;
vector3.x = (Fixed)((Fixed)this.m00 * (Fixed)point.x + (Fixed)this.m01 * (Fixed)point.y + (Fixed)this.m02 * (Fixed)point.z) + this.m03;
vector3.y = (Fixed)((Fixed)this.m10 * (Fixed)point.x + (Fixed)this.m11 * (Fixed)point.y + (Fixed)this.m12 * (Fixed)point.z) + this.m13;
vector3.z = (Fixed)((Fixed)this.m20 * (Fixed)point.x + (Fixed)this.m21 * (Fixed)point.y + (Fixed)this.m22 * (Fixed)point.z) + this.m23;
return(vector3);
}
public static FixedQuaternion AngleAxis(Fixed degree, FixedVector3 axis)
{
if (axis.sqrMagnitude < Fixed.EN6)
{
return(Identity);
}
axis = axis.normalized;
var cos = FixedMathf.Cos(degree * Fixed.Deg2Rad / new Fixed(2));
var sin = FixedMathf.Sin(degree * Fixed.Deg2Rad / new Fixed(2));
return(new FixedQuaternion(sin * axis.x, sin * axis.y, sin * axis.z, cos));
}
/// <summary>
/// <para>Transforms a position by this matrix (generic).</para>
/// </summary>
/// <param name="point"></param>
public FixedVector3 MultiplyPoint(FixedVector3 point)
{
FixedVector3 vector3;
vector3.x = (Fixed)((Fixed)this.m00 * (Fixed)point.x + (Fixed)this.m01 * (Fixed)point.y + (Fixed)this.m02 * (Fixed)point.z) + this.m03;
vector3.y = (Fixed)((Fixed)this.m10 * (Fixed)point.x + (Fixed)this.m11 * (Fixed)point.y + (Fixed)this.m12 * (Fixed)point.z) + this.m13;
vector3.z = (Fixed)((Fixed)this.m20 * (Fixed)point.x + (Fixed)this.m21 * (Fixed)point.y + (Fixed)this.m22 * (Fixed)point.z) + this.m23;
Fixed num = 1f / ((Fixed)((Fixed)this.m30 * (Fixed)point.x + (Fixed)this.m31 * (Fixed)point.y + (Fixed)this.m32 * (Fixed)point.z) + this.m33);
vector3.x *= num;
vector3.y *= num;
vector3.z *= num;
return(vector3);
}
/// <summary>
/// 围绕指定轴旋转
/// </summary>
/// <param name="m">输出矩阵</param>
/// <param name="axis">轴向量</param>
/// <param name="angle">角度</param>
/// <returns></returns>
public static FixedMatrix4x4 MatrixRotation(this FixedMatrix4x4 m, FixedVector3 axis, Fixed angle)
{
m.m00 = axis.x * axis.x * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + FixedMathf.Cos(angle * FixedMathf.Deg2Rad);
m.m01 = axis.x * axis.y * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + axis.z * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m02 = axis.x * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - axis.y * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m10 = axis.x * axis.y * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - axis.z * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m11 = axis.y * axis.y * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + FixedMathf.Cos(angle * FixedMathf.Deg2Rad);
m.m12 = axis.y * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + axis.x * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m20 = axis.x * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - axis.y * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m21 = axis.y * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m22 = axis.z * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + FixedMathf.Cos(angle * FixedMathf.Deg2Rad);
return(m);
}
public static FixedQuaternion Euler(FixedVector3 v)
{
var sinz = FixedMathf.Sin(v.z / 2 * Fixed.Deg2Rad);
var cosz = FixedMathf.Cos(v.z / 2 * Fixed.Deg2Rad);
var sinx = FixedMathf.Sin(v.x / 2 * Fixed.Deg2Rad);
var cosx = FixedMathf.Cos(v.x / 2 * Fixed.Deg2Rad);
var siny = FixedMathf.Sin(v.y / 2 * Fixed.Deg2Rad);
var cosy = FixedMathf.Cos(v.y / 2 * Fixed.Deg2Rad);
return(new FixedQuaternion(
cosy * sinx * cosz + siny * cosx * sinz,
siny * cosx * cosz - cosy * sinx * sinz,
cosy * cosx * sinz - siny * sinx * cosz,
cosy * cosx * cosz + siny * sinx * sinz
));
}
public static FixedQuaternion LookRotation(FixedVector3 forward)
{
FixedVector3 up = FixedVector3.Up;
Fixed f = FixedVector3.Dot(up, forward);
if (f == -1)
{
up = FixedVector3.Forward;
}
else if (f == 1)
{
up = FixedVector3.Back;
}
return(LookRotation(forward, up));
}
public static FixedVector3 Scale(FixedVector3 a, FixedVector3 b)
{
return(new FixedVector3(a.x * b.x, a.y * b.y, a.z * b.z));
}
public static Fixed SqrDistance(FixedVector3 pos, FixedVector3 center)
{
return((pos - center).sqrMagnitude);
}
public static Fixed SqrDistanceWithoutY(FixedVector3 pos, FixedVector3 center)
{
return((pos - center).SqrMagnitudeWithoutY());
}
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 bool Equals(FixedVector3 other)
{
return(x == other.x && y == other.y && z == other.z);
}
public static Fixed Dot(FixedVector3 lhs, FixedVector3 rhs)
{
return(lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z);
}
public static Fixed Angle(FixedVector3 a, FixedVector3 b)
{
return(Vector3.Angle(a.AsVector3(), b.AsVector3()));
}
public static FixedVector3 Lerp(FixedVector3 a, FixedVector3 b, Fixed t)
{
t = FixedMathf.Clamp01(t);
return(new FixedVector3(a.x + (b.x - a.x) * t, a.y + (b.y - a.y) * t, a.z + (b.z - a.z) * t));
}
public static Fixed DistanceIgnoreY(FixedVector3 pos, FixedVector3 center)
{
pos.y = 0;
center.y = 0;
return((pos - center).magnitude);
}
