/// <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);
}
/// <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);
}
/// <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);
}
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>
/// 检查一个target是否在center面前夹角内
/// </summary>
/// <param name="targetPos">目标点</param>
/// <param name="centerPos">中心点</param>
/// <param name="centerForward">中心点方向</param>
/// <param name="angle"></param>
/// <returns></returns>
public static bool CheckInAngle(Vector3 targetPos, Vector3 centerPos, Vector3 centerForward, float angle)
{
Vector3 targetDir = centerForward;
Vector3 dir = Vector3.Normalize(targetPos - centerPos);
float dot = Vector3.Dot(targetDir, dir);
return(dot > FixedMathf.Cos(angle));
}
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>
/// 围绕指定轴旋转
/// </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
));
}
/// <summary>
/// 是否在三角形内
/// </summary>
/// <param name="point"></param>
/// <param name="v0"></param>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static bool isINTriangle(Vector3 point, Vector3 v0, Vector3 v1, Vector3 v2)
{
float x = point.x;
float y = point.z;
float v0x = v0.x;
float v0y = v0.z;
float v1x = v1.x;
float v1y = v1.z;
float v2x = v2.x;
float v2y = v2.z;
float t = triangleArea(v0x, v0y, v1x, v1y, v2x, v2y);
float a = triangleArea(v0x, v0y, v1x, v1y, x, y) + triangleArea(v0x, v0y, x, y, v2x, v2y) + triangleArea(x, y, v1x, v1y, v2x, v2y);
if (FixedMathf.Abs(t - a) <= 0.01f)
{
return(true);
}
else
{
return(false);
}
}
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);
}
/// <summary>
/// <para>Linearly interpolates between vectors a and b by t.</para>
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
public static FixedVector2 Lerp(FixedVector2 a, FixedVector2 b, Fixed t)
{
t = FixedMathf.Clamp01(t);
return(new FixedVector2(a.x + (b.x - a.x) * t, a.y + (b.y - a.y) * t));
}
/// <summary>
/// <para>Returns a point inside a rectangle, given normalized coordinates.</para>
/// </summary>
/// <param name="rectangle">Rectangle to get a point inside.</param>
/// <param name="normalizedRectCoordinates">Normalized coordinates to get a point for.</param>
public static FixedVector2 NormalizedToPoint(FixedRect rectangle, FixedVector2 normalizedRectCoordinates)
{
return(new FixedVector2(FixedMathf.Lerp(rectangle.x, rectangle.xMax, normalizedRectCoordinates.x), FixedMathf.Lerp(rectangle.y, rectangle.yMax, normalizedRectCoordinates.y)));
}
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));
}
