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