From 826e80c5d89858d813bb8c112f02ad7811eb8336 Mon Sep 17 00:00:00 2001 From: Sardelka9515 Date: Sun, 26 Mar 2023 12:03:06 +0800 Subject: [PATCH] Reintroduce compact vector types --- Core/CompactVectors/LQuaternion.cs | 968 +++++++++++++++++++++++++++++ Core/CompactVectors/LVector2.cs | 512 +++++++++++++++ Core/CompactVectors/LVector3.cs | 731 ++++++++++++++++++++++ 3 files changed, 2211 insertions(+) create mode 100644 Core/CompactVectors/LQuaternion.cs create mode 100644 Core/CompactVectors/LVector2.cs create mode 100644 Core/CompactVectors/LVector3.cs diff --git a/Core/CompactVectors/LQuaternion.cs b/Core/CompactVectors/LQuaternion.cs new file mode 100644 index 00000000..6733201c --- /dev/null +++ b/Core/CompactVectors/LQuaternion.cs @@ -0,0 +1,968 @@ +using System; +using System.Collections.Generic; +using System.Globalization; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using GTA.Math; + +namespace RageCoop.Core.CompactVectors +{ + + public struct LQuaternion : IEquatable + { + /// + /// Gets or sets the X component of the quaternion. + /// + /// The X component of the quaternion. + public float X; + + /// + /// Gets or sets the Y component of the quaternion. + /// + /// The Y component of the quaternion. + public float Y; + + /// + /// Gets or sets the Z component of the quaternion. + /// + /// The Z component of the quaternion. + public float Z; + + /// + /// Gets or sets the W component of the quaternion. + /// + /// The W component of the quaternion. + public float W; + + /// + /// Initializes a new instance of the structure. + /// + /// The X component of the quaternion. + /// The Y component of the quaternion. + /// The Z component of the quaternion. + /// The W component of the quaternion. + public LQuaternion(float x, float y, float z, float w) : this() + { + X = x; + Y = y; + Z = z; + W = w; + } + + /// + /// Initializes a new instance of the structure. + /// + /// The axis of rotation. + /// The angle of rotation in radians. + public LQuaternion(Vector3 axis, float angle) : this() + { + axis = Vector3.Normalize(axis); + + float half = angle * 0.5f; + float sin = (float)(System.Math.Sin((double)(half))); + float cos = (float)(System.Math.Cos((double)(half))); + + X = axis.X * sin; + Y = axis.Y * sin; + Z = axis.Z * sin; + W = cos; + } + + /// + /// A with all of its components set to zero. + /// + public static LQuaternion Zero => new LQuaternion(); + + /// + /// A with all of its components set to one. + /// + public static LQuaternion One => new LQuaternion(1.0f, 1.0f, 1.0f, 1.0f); + + /// + /// The identity (0, 0, 0, 1). + /// + public static LQuaternion Identity => new LQuaternion(0.0f, 0.0f, 0.0f, 1.0f); + + /// + /// Gets the axis components of the quaternion. + /// + public Vector3 Axis + { + get + { + if (Length() != 1.0f) + { + return Vector3.Zero; + } + + float length = 1.0f - (W * W); + if (length == 0f) + { + return Vector3.UnitX; + } + + float inv = 1.0f / (float)System.Math.Sqrt(length); + return new Vector3(X * inv, Y * inv, Z * inv); + } + } + + /// + /// Gets the angle of the quaternion. + /// + public float Angle => ((System.Math.Abs(W) <= 1.0f) ? 2.0f * (float)(System.Math.Acos(W)) : 0.0f); + + /// + /// Calculates the length of the quaternion. + /// + /// The length of the quaternion. + public float Length() => (float)System.Math.Sqrt((X * X) + (Y * Y) + (Z * Z) + (W * W)); + + /// + /// Calculates the squared length of the quaternion. + /// + /// The squared length of the quaternion. + public float LengthSquared() => (X * X) + (Y * Y) + (Z * Z) + (W * W); + + /// + /// Converts the quaternion into a unit quaternion. + /// + public void Normalize() + { + float length = Length(); + if (length != 0f) + { + float inverse = 1.0f / length; + X *= inverse; + Y *= inverse; + Z *= inverse; + W *= inverse; + } + } + + /// + /// Conjugates the quaternion. + /// + public void Conjugate() + { + X = -X; + Y = -Y; + Z = -Z; + } + + /// + /// Conjugates and renormalizes the quaternion. + /// + public void Invert() + { + float lengthSq = LengthSquared(); + if (lengthSq != 0f) + { + lengthSq = 1.0f / lengthSq; + + X = -X * lengthSq; + Y = -Y * lengthSq; + Z = -Z * lengthSq; + W = W * lengthSq; + } + } + + /// + /// Reverses the direction of a given quaternion. + /// + /// The quaternion to negate. + /// A quaternion facing in the opposite direction. + public static LQuaternion Negate(LQuaternion quaternion) + { + LQuaternion result = Zero; + result.X = -quaternion.X; + result.Y = -quaternion.Y; + result.Z = -quaternion.Z; + result.W = -quaternion.W; + return result; + } + + /// + /// Adds two quaternions. + /// + /// The first quaternion to add. + /// The second quaternion to add. + /// The sum of the two quaternions. + public static LQuaternion Add(LQuaternion left, LQuaternion right) + { + LQuaternion result = Zero; + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + result.W = left.W + right.W; + return result; + } + + /// + /// Subtracts two quaternions. + /// + /// The first quaternion to subtract. + /// The second quaternion to subtract. + /// The difference of the two quaternions. + public static LQuaternion Subtract(LQuaternion left, LQuaternion right) + { + LQuaternion result = Zero; + result.X = left.X - right.X; + result.Y = left.Y - right.Y; + result.Z = left.Z - right.Z; + result.W = left.W - right.W; + return result; + } + + /// + /// Multiplies two Quaternions together. + /// + /// The Quaternion on the left side of the multiplication. + /// The Quaternion on the right side of the multiplication. + /// The result of the multiplication. + public static LQuaternion Multiply(LQuaternion left, LQuaternion right) + { + LQuaternion quaternion; + float lx = left.X; + float ly = left.Y; + float lz = left.Z; + float lw = left.W; + float rx = right.X; + float ry = right.Y; + float rz = right.Z; + float rw = right.W; + + quaternion.X = (lx * rw + rx * lw) + (ly * rz) - (lz * ry); + quaternion.Y = (ly * rw + ry * lw) + (lz * rx) - (lx * rz); + quaternion.Z = (lz * rw + rz * lw) + (lx * ry) - (ly * rx); + quaternion.W = (lw * rw) - (lx * rx + ly * ry + lz * rz); + + return quaternion; + } + + /// + /// Scales a quaternion by the given value. + /// + /// The quaternion to scale. + /// The amount by which to scale the quaternion. + /// The scaled quaternion. + public static LQuaternion Multiply(LQuaternion quaternion, float scale) + { + LQuaternion result = Zero; + result.X = quaternion.X * scale; + result.Y = quaternion.Y * scale; + result.Z = quaternion.Z * scale; + result.W = quaternion.W * scale; + return result; + } + + /// + /// Divides a quaternion by another. + /// + /// The first quaternion to divide. + /// The second quaternion to divide. + /// The divided quaternion. + public static LQuaternion Divide(LQuaternion left, LQuaternion right) + { + return Multiply(left, Invert(right)); + } + + /// + /// Converts the quaternion into a unit quaternion. + /// + /// The quaternion to normalize. + /// The normalized quaternion. + public static LQuaternion Normalize(LQuaternion quaternion) + { + quaternion.Normalize(); + return quaternion; + } + + /// + /// Creates the conjugate of a specified Quaternion. + /// + /// The Quaternion of which to return the conjugate. + /// A new Quaternion that is the conjugate of the specified one. + public static LQuaternion Conjugate(LQuaternion value) + { + LQuaternion ans; + + ans.X = -value.X; + ans.Y = -value.Y; + ans.Z = -value.Z; + ans.W = value.W; + + return ans; + } + + /// + /// Conjugates and renormalizes the quaternion. + /// + /// The quaternion to conjugate and re-normalize. + /// The conjugated and renormalized quaternion. + public static LQuaternion Invert(LQuaternion quaternion) + { + LQuaternion result = Zero; + float lengthSq = 1.0f / ((quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W)); + + result.X = -quaternion.X * lengthSq; + result.Y = -quaternion.Y * lengthSq; + result.Z = -quaternion.Z * lengthSq; + result.W = quaternion.W * lengthSq; + + return result; + } + + /// + /// Calculates the dot product of two quaternions. + /// + /// First source quaternion. + /// Second source quaternion. + /// The dot product of the two quaternions. + public static float Dot(LQuaternion left, LQuaternion right) => (left.X * right.X) + (left.Y * right.Y) + (left.Z * right.Z) + (left.W * right.W); + + /// + /// Performs a linear interpolation between two quaternion. + /// + /// Start quaternion. + /// End quaternion. + /// Value between 0 and 1 indicating the weight of . + /// The linear interpolation of the two quaternions. + /// + /// This method performs the linear interpolation based on the following formula. + /// start + (end - start) * amount + /// Passing a value of 0 will cause to be returned; a value of 1 will cause to be returned. + /// + public static LQuaternion Lerp(LQuaternion start, LQuaternion end, float amount) + { + LQuaternion result = Zero; + float inverse = 1.0f - amount; + float dot = (start.X * end.X) + (start.Y * end.Y) + (start.Z * end.Z) + (start.W * end.W); + + if (dot >= 0.0f) + { + result.X = (inverse * start.X) + (amount * end.X); + result.Y = (inverse * start.Y) + (amount * end.Y); + result.Z = (inverse * start.Z) + (amount * end.Z); + result.W = (inverse * start.W) + (amount * end.W); + } + else + { + result.X = (inverse * start.X) - (amount * end.X); + result.Y = (inverse * start.Y) - (amount * end.Y); + result.Z = (inverse * start.Z) - (amount * end.Z); + result.W = (inverse * start.W) - (amount * end.W); + } + + float invLength = 1.0f / result.Length(); + + result.X *= invLength; + result.Y *= invLength; + result.Z *= invLength; + result.W *= invLength; + + return result; + } + + /// + /// Interpolates between two quaternions, using spherical linear interpolation.. + /// + /// Start quaternion. + /// End quaternion. + /// Value between 0 and 1 indicating the weight of . + /// The spherical linear interpolation of the two quaternions. + public static LQuaternion Slerp(LQuaternion start, LQuaternion end, float amount) + { + LQuaternion result = Zero; + float kEpsilon = (float)(1.192093E-07); + float opposite; + float inverse; + float dot = Dot(start, end); + + if (System.Math.Abs(dot) > (1.0f - kEpsilon)) + { + inverse = 1.0f - amount; + opposite = amount * System.Math.Sign(dot); + } + else + { + float acos = (float)System.Math.Acos(System.Math.Abs(dot)); + float invSin = (float)(1.0 / System.Math.Sin(acos)); + + inverse = (float)(System.Math.Sin((1.0f - amount) * acos) * invSin); + opposite = (float)(System.Math.Sin(amount * acos) * invSin * System.Math.Sign(dot)); + } + + result.X = (inverse * start.X) + (opposite * end.X); + result.Y = (inverse * start.Y) + (opposite * end.Y); + result.Z = (inverse * start.Z) + (opposite * end.Z); + result.W = (inverse * start.W) + (opposite * end.W); + + return result; + } + + /// + /// Interpolates between two quaternions, using spherical linear interpolation. The parameter /t/ is not clamped. + /// + /// + /// + /// + public static LQuaternion SlerpUnclamped(LQuaternion a, LQuaternion b, float t) + { + if (a.LengthSquared() == 0.0f) + { + if (b.LengthSquared() == 0.0f) + { + return Identity; + } + return b; + } + else if (b.LengthSquared() == 0.0f) + { + return a; + } + + + float cosHalfAngle = a.W * b.W + Vector3.Dot(a.Axis, b.Axis); + + if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) + { + return a; + } + else if (cosHalfAngle < 0.0f) + { + b.X = -b.X; + b.Y = -b.Y; + b.Z = -b.Z; + b.W = -b.W; + cosHalfAngle = -cosHalfAngle; + } + + float blendA; + float blendB; + if (cosHalfAngle < 0.99f) + { + float halfAngle = (float)System.Math.Acos(cosHalfAngle); + float sinHalfAngle = (float)System.Math.Sin(halfAngle); + float oneOverSinHalfAngle = 1.0f / sinHalfAngle; + blendA = (float)System.Math.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle; + blendB = (float)System.Math.Sin(halfAngle * t) * oneOverSinHalfAngle; + } + else + { + blendA = 1.0f - t; + blendB = t; + } + + LQuaternion result = new LQuaternion(blendA * a.Axis + blendB * b.Axis, blendA * a.W + blendB * b.W); + if (result.LengthSquared() > 0.0f) + { + return Normalize(result); + } + else + { + return Identity; + } + } + + /// + /// Creates a rotation with the specified and directions. + /// + public static LQuaternion LookRotation(Vector3 forward) => LookRotation(forward, Vector3.WorldUp); + /// + /// Creates a rotation with the specified and directions. + /// + public static LQuaternion LookRotation(Vector3 forward, Vector3 up) => DirectionVectors(Vector3.Cross(forward, up), forward, up); + + /// + /// Creates a rotation which rotates from fromDirection to toDirection. + /// + public static LQuaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection) + { + float NormAB = (float)(System.Math.Sqrt(fromDirection.LengthSquared() * fromDirection.LengthSquared())); + + float w = NormAB + Vector3.Dot(fromDirection, toDirection); + LQuaternion Result; + + if (w >= 1e-6f * NormAB) + { + Result = new LQuaternion(Vector3.Cross(fromDirection, toDirection), w); + } + else + { + w = 0.0f; + Result = System.Math.Abs(fromDirection.X) > System.Math.Abs(fromDirection.Y) + ? new LQuaternion(-fromDirection.Z, 0.0f, fromDirection.X, w) + : new LQuaternion(0.0f, -fromDirection.Z, fromDirection.Y, w); + } + + Result.Normalize(); + return Result; + } + + /// + /// Rotates a rotation from towards to. + /// + /// From Quaternion. + /// To Quaternion. + /// + public static LQuaternion RotateTowards(LQuaternion from, LQuaternion to, float maxDegreesDelta) + { + float angle = AngleBetween(from, to); + if (angle == 0.0f) + { + return to; + } + float t = System.Math.Min(1.0f, maxDegreesDelta / angle); + return SlerpUnclamped(from, to, t); + } + + /// + /// Returns the angle in degrees between two rotations a and b. + /// + /// The first quaternion to calculate angle. + /// The second quaternion to calculate angle. + /// The angle in degrees between two rotations a and b. + public static float AngleBetween(LQuaternion a, LQuaternion b) + { + float dot = Dot(a, b); + return (float)((System.Math.Acos(System.Math.Min(System.Math.Abs(dot), 1.0f)) * 2.0 * (180.0f / System.Math.PI))); + } + + /// + /// Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order). + /// + /// Z degrees. + /// X degrees. + /// Y degrees. + public static LQuaternion Euler(float zaxis, float xaxis, float yaxis) + { + float Deg2Rad = (float)((System.Math.PI / 180.0)); + return RotationYawPitchRoll(zaxis * Deg2Rad, xaxis * Deg2Rad, yaxis * Deg2Rad); + } + + /// + /// Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order). + /// + /// Euler angles in degrees. euler.X = around X axis, euler.Y = around Y axis, euler.Z = around Z axis + public static LQuaternion Euler(Vector3 euler) + { + Vector3 eulerRad = euler * (float)((System.Math.PI / 180.0)); + return RotationYawPitchRoll(eulerRad.Z, eulerRad.X, eulerRad.Y); + } + + /// + /// Creates a quaternion given a rotation and an axis. + /// + /// The axis of rotation. + /// The angle of rotation in radians. + /// The newly created quaternion. + public static LQuaternion RotationAxis(Vector3 axis, float angle) + { + LQuaternion result = Zero; + + axis = Vector3.Normalize(axis); + + float half = angle * 0.5f; + float sin = (float)(System.Math.Sin((double)(half))); + float cos = (float)(System.Math.Cos((double)(half))); + + result.X = axis.X * sin; + result.Y = axis.Y * sin; + result.Z = axis.Z * sin; + result.W = cos; + + return result; + } + + /// + /// Creates a quaternion given a rotation matrix. + /// + /// The rotation matrix. + /// The newly created quaternion. + public static LQuaternion RotationMatrix(Matrix matrix) + { + LQuaternion result = Zero; + float sqrt; + float half; + float scale = matrix.M11 + matrix.M22 + matrix.M33; + + if (scale > 0.0f) + { + sqrt = (float)System.Math.Sqrt(scale + 1.0f); + result.W = sqrt * 0.5f; + sqrt = 0.5f / sqrt; + + result.X = (matrix.M23 - matrix.M32) * sqrt; + result.Y = (matrix.M31 - matrix.M13) * sqrt; + result.Z = (matrix.M12 - matrix.M21) * sqrt; + } + else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33)) + { + sqrt = (float)System.Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33); + half = 0.5f / sqrt; + + result.X = 0.5f * sqrt; + result.Y = (matrix.M12 + matrix.M21) * half; + result.Z = (matrix.M13 + matrix.M31) * half; + result.W = (matrix.M23 - matrix.M32) * half; + } + else if (matrix.M22 > matrix.M33) + { + sqrt = (float)System.Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33); + half = 0.5f / sqrt; + + result.X = (matrix.M21 + matrix.M12) * half; + result.Y = 0.5f * sqrt; + result.Z = (matrix.M32 + matrix.M23) * half; + result.W = (matrix.M31 - matrix.M13) * half; + } + else + { + sqrt = (float)System.Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22); + half = 0.5f / sqrt; + + result.X = (matrix.M31 + matrix.M13) * half; + result.Y = (matrix.M32 + matrix.M23) * half; + result.Z = 0.5f * sqrt; + result.W = (matrix.M12 - matrix.M21) * half; + } + + return result; + } + + /// + /// Creates a Quaternion from the given yaw, pitch, and roll, in radians. + /// + /// The yaw angle, in radians, around the Z-axis. + /// The pitch angle, in radians, around the X-axis. + /// The roll angle, in radians, around the Y-axis. + /// The newly created quaternion. + public static LQuaternion RotationYawPitchRoll(float yaw, float pitch, float roll) + { + LQuaternion result = Zero; + + float halfYaw = yaw * 0.5f; + float sinYaw = (float)(System.Math.Sin((double)(halfYaw))); + float cosYaw = (float)(System.Math.Cos((double)(halfYaw))); + float halfPitch = pitch * 0.5f; + float sinPitch = (float)(System.Math.Sin((double)(halfPitch))); + float cosPitch = (float)(System.Math.Cos((double)(halfPitch))); + float halfRoll = roll * 0.5f; + float sinRoll = (float)(System.Math.Sin((double)(halfRoll))); + float cosRoll = (float)(System.Math.Cos((double)(halfRoll))); + + result.X = (cosRoll * sinPitch * cosYaw) + (sinRoll * cosPitch * sinYaw); + result.Y = (sinRoll * cosPitch * cosYaw) - (cosRoll * sinPitch * sinYaw); + result.Z = (cosRoll * cosPitch * sinYaw) - (sinRoll * sinPitch * cosYaw); + result.W = (cosRoll * cosPitch * cosYaw) + (sinRoll * sinPitch * sinYaw); + + return result; + } + + /// + /// Creates a Quaternion from the given relative x, y, z axis + /// + /// The Vectors need to be perpendicular to each other + /// Relative X axis + /// Relative Y axis + /// Relative Z axis + /// The newly created quaternion. + public static LQuaternion DirectionVectors(Vector3 rightVector, Vector3 forwardVector, Vector3 upVector) + { + rightVector.Normalize(); + forwardVector.Normalize(); + upVector.Normalize(); + + Matrix rotationMatrix = new Matrix(); + rotationMatrix[0, 0] = rightVector.X; + rotationMatrix[0, 1] = rightVector.Y; + rotationMatrix[0, 2] = rightVector.Z; + + rotationMatrix[1, 0] = forwardVector.X; + rotationMatrix[1, 1] = forwardVector.Y; + rotationMatrix[1, 2] = forwardVector.Z; + + rotationMatrix[2, 0] = upVector.X; + rotationMatrix[2, 1] = upVector.Y; + rotationMatrix[2, 2] = upVector.Z; + + return RotationMatrix(rotationMatrix); + } + + /// + /// Get direction vectors from the given quaternion + /// + /// The quaternion + /// RightVector = relative x axis + /// ForwardVector = relative y axis + /// UpVector = relative z axis + public static void GetDirectionVectors(LQuaternion quaternion, out Vector3 rightVector, out Vector3 forwardVector, out Vector3 upVector) + { + quaternion.Normalize(); + rightVector = quaternion * Vector3.WorldEast; + forwardVector = quaternion * Vector3.WorldNorth; + upVector = quaternion * Vector3.WorldUp; + } + + /// + /// Reverses the direction of a given quaternion. + /// + /// The quaternion to negate. + /// A quaternion facing in the opposite direction. + public static LQuaternion operator -(LQuaternion quaternion) + { + LQuaternion result = Zero; + result.X = -quaternion.X; + result.Y = -quaternion.Y; + result.Z = -quaternion.Z; + result.W = -quaternion.W; + return result; + } + + /// + /// Adds two quaternions. + /// + /// The first quaternion to add. + /// The second quaternion to add. + /// The sum of the two quaternions. + public static LQuaternion operator +(LQuaternion left, LQuaternion right) + { + LQuaternion result = Zero; + result.X = left.X + right.X; + result.Y = left.Y + right.Y; + result.Z = left.Z + right.Z; + result.W = left.W + right.W; + return result; + } + + /// + /// Subtracts two quaternions. + /// + /// The first quaternion to subtract. + /// The second quaternion to subtract. + /// The difference of the two quaternions. + public static LQuaternion operator -(LQuaternion left, LQuaternion right) + { + LQuaternion result = Zero; + result.X = left.X - right.X; + result.Y = left.Y - right.Y; + result.Z = left.Z - right.Z; + result.W = left.W - right.W; + return result; + } + + /// + /// Multiplies a quaternion by another. + /// + /// The first quaternion to multiply. + /// The second quaternion to multiply. + /// The multiplied quaternion. + public static LQuaternion operator *(LQuaternion left, LQuaternion right) + { + LQuaternion quaternion = Zero; + float lx = left.X; + float ly = left.Y; + float lz = left.Z; + float lw = left.W; + float rx = right.X; + float ry = right.Y; + float rz = right.Z; + float rw = right.W; + + quaternion.X = (lx * rw + rx * lw) + (ly * rz) - (lz * ry); + quaternion.Y = (ly * rw + ry * lw) + (lz * rx) - (lx * rz); + quaternion.Z = (lz * rw + rz * lw) + (lx * ry) - (ly * rx); + quaternion.W = (lw * rw) - (lx * rx + ly * ry + lz * rz); + + return quaternion; + } + + /// + /// Scales a quaternion by the given value. + /// + /// The quaternion to scale. + /// The amount by which to scale the quaternion. + /// The scaled quaternion. + public static LQuaternion operator *(LQuaternion quaternion, float scale) + { + LQuaternion result = Zero; + result.X = quaternion.X * scale; + result.Y = quaternion.Y * scale; + result.Z = quaternion.Z * scale; + result.W = quaternion.W * scale; + return result; + } + + /// + /// Scales a quaternion by the given value. + /// + /// The quaternion to scale. + /// The amount by which to scale the quaternion. + /// The scaled quaternion. + public static LQuaternion operator *(float scale, LQuaternion quaternion) + { + LQuaternion result = Zero; + result.X = quaternion.X * scale; + result.Y = quaternion.Y * scale; + result.Z = quaternion.Z * scale; + result.W = quaternion.W * scale; + return result; + } + + /// + /// Divides a Quaternion by another Quaternion. + /// + /// The source Quaternion. + /// The divisor. + /// The result of the division. + public static LQuaternion operator /(LQuaternion left, LQuaternion right) + { + LQuaternion quaternion = Zero; + + float lx = left.X; + float ly = left.Y; + float lz = left.Z; + float lw = left.W; + + // Inverse part. + float ls = right.X * right.X + right.Y * right.Y + + right.Z * right.Z + right.W * right.W; + float invNorm = 1.0f / ls; + + float rx = -right.X * invNorm; + float ry = -right.Y * invNorm; + float rz = -right.Z * invNorm; + float rw = right.W * invNorm; + + // Multiply part. + quaternion.X = (lx * rw + rx * lw) + (ly * rz) - (lz * ry); + quaternion.Y = (ly * rw + ry * lw) + (lz * rx) - (lx * rz); + quaternion.Z = (lz * rw + rz * lw) + (lx * ry) - (ly * rx); + quaternion.W = (lw * rw) - (lx * rx + ly * ry + lz * rz); + + return quaternion; + } + + /// + /// Tests for equality between two objects. + /// + /// The first value to compare. + /// The second value to compare. + /// if has the same value as ; otherwise, . + public static bool operator ==(LQuaternion left, LQuaternion right) => Equals(left, right); + + /// + /// Tests for inequality between two objects. + /// + /// The first value to compare. + /// The second value to compare. + /// if has a different value than ; otherwise, . + public static bool operator !=(LQuaternion left, LQuaternion right) => !Equals(left, right); + + #region RotateTransformOperators + + /// + /// Rotates the point with rotation. + /// + /// The quaternion to rotate the vector. + /// The vector to be rotated. + /// The vector after rotation. + public static Vector3 operator *(LQuaternion rotation, Vector3 point) + { + float q0 = rotation.W; + float q0Square = rotation.W * rotation.W; + Vector3 q = new Vector3(rotation.X, rotation.Y, rotation.Z); + return ((q0Square - q.LengthSquared()) * point) + (2 * Vector3.Dot(q, point) * q) + (2 * q0 * Vector3.Cross(q, point)); + } + + /// + /// Rotates the point with rotation. + /// + /// The quaternion to rotate the vector. + /// The vector to be rotated. + /// The vector after rotation. + public static Vector3 RotateTransform(LQuaternion rotation, Vector3 point) => rotation * point; + + /// + /// Rotates the point with rotation. + /// + /// The quaternion to rotate the vector. + /// The vector to be rotated. + /// The vector representing the origin of the new coordinate system. + /// The vector after rotation in the original coordinate system. + public static Vector3 RotateTransform(LQuaternion rotation, Vector3 point, Vector3 center) + { + Vector3 PointNewCenter = Vector3.Subtract(point, center); + Vector3 TransformedPoint = RotateTransform(rotation, PointNewCenter); + return Vector3.Add(TransformedPoint, center); + } + + /// + /// Rotates the point with rotation. + /// + /// The vector to be rotated. + /// The vector after rotation. + public Vector3 RotateTransform(Vector3 point) => RotateTransform(this, point); + + /// + /// Rotates the point with rotation. + /// + /// The vector to be rotated. + /// The vector representing the origin of the new coordinate system. + /// The vector after rotation in the original coordinate system. + public Vector3 RotateTransform(Vector3 point, Vector3 center) => RotateTransform(this, point, center); + + #endregion RotateTransformOperators + + /// + /// Converts the value of the object to its equivalent string representation. + /// + /// The string representation of the value of this instance. + public override string ToString() + { + return String.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString(), Y.ToString(), Z.ToString(), W.ToString()); + } + + /// + /// Converts the value of the object to its equivalent string representation. + /// + /// The format. + /// The string representation of the value of this instance. + public string ToString(string format) + { + return String.Format(CultureInfo.InvariantCulture, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString(format), Y.ToString(format), Z.ToString(format), W.ToString(format)); + } + + /// + /// Returns the hash code for this instance. + /// + /// A 32-bit signed integer hash code. + public override int GetHashCode() => X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode(); + + /// + /// Returns a value that indicates whether the current instance is equal to a specified object. + /// + /// Object to make the comparison with. + /// if the current instance is equal to the specified object; otherwise. + public override bool Equals(object obj) + { + if (obj == null || obj.GetType() != GetType()) + { + return false; + } + + return Equals((LQuaternion)obj); + } + + /// + /// Returns a value that indicates whether the current instance is equal to the specified object. + /// + /// Object to make the comparison with. + /// if the current instance is equal to the specified object; otherwise. + public bool Equals(LQuaternion other) => (X == other.X && Y == other.Y && Z == other.Z && W == other.W); + + public static implicit operator Quaternion(LQuaternion q) => new(q.X, q.Y, q.Z, q.W); + public static implicit operator LQuaternion(Quaternion q) => new(q.X, q.Y, q.Z, q.W); + } +} diff --git a/Core/CompactVectors/LVector2.cs b/Core/CompactVectors/LVector2.cs new file mode 100644 index 00000000..8202ca67 --- /dev/null +++ b/Core/CompactVectors/LVector2.cs @@ -0,0 +1,512 @@ +using System; +using System.Collections.Generic; +using System.Globalization; +using GTA.Math; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RageCoop.Core +{ + public struct LVector2 : IEquatable + { + /// + /// Gets or sets the X component of the vector. + /// + /// The X component of the vector. + public float X; + + /// + /// Gets or sets the Y component of the vector. + /// + /// The Y component of the vector. + public float Y; + + /// + /// Initializes a new instance of the class. + /// + /// Initial value for the X component of the vector. + /// Initial value for the Y component of the vector. + public LVector2(float x, float y) + { + X = x; + Y = y; + } + + /// + /// Returns this vector with a magnitude of 1. + /// + public LVector2 Normalized => Normalize(new LVector2(X, Y)); + + /// + /// Returns a null vector. (0,0) + /// + public static LVector2 Zero => new LVector2(0.0f, 0.0f); + + /// + /// The X unit (1, 0). + /// + public static LVector2 UnitX => new LVector2(1.0f, 0.0f); + + /// + /// The Y unit (0, 1). + /// + public static LVector2 UnitY => new LVector2(0.0f, 1.0f); + + /// + /// Returns the up vector. (0,1) + /// + public static LVector2 Up => new LVector2(0.0f, 1.0f); + + /// + /// Returns the down vector. (0,-1) + /// + public static LVector2 Down => new LVector2(0.0f, -1.0f); + + /// + /// Returns the right vector. (1,0) + /// + public static LVector2 Right => new LVector2(1.0f, 0.0f); + + /// + /// Returns the left vector. (-1,0) + /// + public static LVector2 Left => new LVector2(-1.0f, 0.0f); + + /// + /// Gets or sets the component at the specified index. + /// + /// The value of the X or Y component, depending on the index. + /// The index of the component to access. Use 0 for the X component and 1 for the Y component. + /// The value of the component at the specified index. + /// Thrown when the is out of the range [0, 1]. + public float this[int index] + { + get + { + switch (index) + { + case 0: + return X; + case 1: + return Y; + } + + throw new ArgumentOutOfRangeException("index", "Indices for Vector2 run from 0 to 1, inclusive."); + } + + set + { + switch (index) + { + case 0: + X = value; + break; + case 1: + Y = value; + break; + default: + throw new ArgumentOutOfRangeException("index", "Indices for Vector2 run from 0 to 1, inclusive."); + } + } + } + + /// + /// Calculates the length of the vector. + /// + /// The length of the vector. + public float Length() + { + return (float)System.Math.Sqrt((X * X) + (Y * Y)); + } + + /// + /// Calculates the squared length of the vector. + /// + /// The squared length of the vector. + public float LengthSquared() + { + return (X * X) + (Y * Y); + } + + /// + /// Converts the vector into a unit vector. + /// + public void Normalize() + { + float length = Length(); + if (length == 0) + { + return; + } + + float num = 1 / length; + X *= num; + Y *= num; + } + + /// + /// Calculates the distance between two vectors. + /// + /// The second vector to calculate the distance to. + /// The distance to the other vector. + public float DistanceTo(LVector2 position) + { + return (position - this).Length(); + } + + /// + /// Calculates the squared distance between two vectors. + /// + /// The second vector to calculate the squared distance to. + /// The squared distance to the other vector. + public float DistanceToSquared(LVector2 position) + { + return DistanceSquared(position, this); + } + + /// + /// Calculates the distance between two vectors. + /// + /// The first vector to calculate the distance to the second vector. + /// The second vector to calculate the distance to the first vector. + /// The distance between the two vectors. + public static float Distance(LVector2 position1, LVector2 position2) + { + return (position1 - position2).Length(); + } + + /// + /// Calculates the squared distance between two vectors. + /// + /// The first vector to calculate the squared distance to the second vector. + /// The second vector to calculate the squared distance to the first vector. + /// The squared distance between the two vectors. + public static float DistanceSquared(LVector2 position1, LVector2 position2) + { + return (position1 - position2).LengthSquared(); + } + + /// + /// Returns the angle in degrees between from and to. + /// The angle returned is always the acute angle between the two vectors. + /// + public static float Angle(LVector2 from, LVector2 to) + { + return System.Math.Abs(SignedAngle(from, to)); + } + + /// + /// Returns the signed angle in degrees between from and to. + /// + public static float SignedAngle(LVector2 from, LVector2 to) + { + return (float)((System.Math.Atan2(to.Y, to.X) - System.Math.Atan2(from.Y, from.X)) * (180.0 / System.Math.PI)); + } + + /// + /// Converts a vector to a heading. + /// + public float ToHeading() + { + return (float)((System.Math.Atan2(X, -Y) + System.Math.PI) * (180.0 / System.Math.PI)); + } + + /// + /// Returns a new normalized vector with random X and Y components. + /// + public static LVector2 RandomXY() + { + LVector2 v; + double radian = CoreUtils.SafeRandom.NextDouble() * 2 * System.Math.PI; + v.X = (float)(System.Math.Cos(radian)); + v.Y = (float)(System.Math.Sin(radian)); + v.Normalize(); + return v; + } + + /// + /// Adds two vectors. + /// + /// The first vector to add. + /// The second vector to add. + /// The sum of the two vectors. + public static LVector2 Add(LVector2 left, LVector2 right) => new LVector2(left.X + right.X, left.Y + right.Y); + + /// + /// Subtracts two vectors. + /// + /// The first vector to subtract. + /// The second vector to subtract. + /// The difference of the two vectors. + public static LVector2 Subtract(LVector2 left, LVector2 right) => new LVector2(left.X - right.X, left.Y - right.Y); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector2 Multiply(LVector2 value, float scale) => new LVector2(value.X * scale, value.Y * scale); + + /// + /// Multiplies a vector with another by performing component-wise multiplication. + /// + /// The first vector to multiply. + /// The second vector to multiply. + /// The multiplied vector. + public static LVector2 Multiply(LVector2 left, LVector2 right) => new LVector2(left.X * right.X, left.Y * right.Y); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector2 Divide(LVector2 value, float scale) => new LVector2(value.X / scale, value.Y / scale); + + /// + /// Reverses the direction of a given vector. + /// + /// The vector to negate. + /// A vector facing in the opposite direction. + public static LVector2 Negate(LVector2 value) => new LVector2(-value.X, -value.Y); + + /// + /// Restricts a value to be within a specified range. + /// + /// The value to clamp. + /// The minimum value. + /// The maximum value. + /// The clamped value. + public static LVector2 Clamp(LVector2 value, LVector2 min, LVector2 max) + { + float x = value.X; + x = (x > max.X) ? max.X : x; + x = (x < min.X) ? min.X : x; + + float y = value.Y; + y = (y > max.Y) ? max.Y : y; + y = (y < min.Y) ? min.Y : y; + + return new LVector2(x, y); + } + + /// + /// Performs a linear interpolation between two vectors. + /// + /// Start vector. + /// End vector. + /// Value between 0 and 1 indicating the weight of . + /// The linear interpolation of the two vectors. + /// + /// This method performs the linear interpolation based on the following formula. + /// start + (end - start) * amount + /// Passing a value of 0 will cause to be returned; a value of 1 will cause to be returned. + /// + public static LVector2 Lerp(LVector2 start, LVector2 end, float amount) + { + LVector2 vector; + + vector.X = start.X + ((end.X - start.X) * amount); + vector.Y = start.Y + ((end.Y - start.Y) * amount); + + return vector; + } + + /// + /// Converts the vector into a unit vector. + /// + /// The vector to normalize. + /// The normalized vector. + public static LVector2 Normalize(LVector2 vector) + { + vector.Normalize(); + return vector; + } + + /// + /// Calculates the dot product of two vectors. + /// + /// First source vector. + /// Second source vector. + /// The dot product of the two vectors. + public static float Dot(LVector2 left, LVector2 right) => (left.X * right.X + left.Y * right.Y); + + /// + /// Returns the reflection of a vector off a surface that has the specified normal. + /// + /// The source vector. + /// Normal of the surface. + /// The reflected vector. + /// Reflect only gives the direction of a reflection off a surface, it does not determine + /// whether the original vector was close enough to the surface to hit it. + public static LVector2 Reflect(LVector2 vector, LVector2 normal) + { + LVector2 result; + float dot = ((vector.X * normal.X) + (vector.Y * normal.Y)); + + result.X = vector.X - ((2.0f * dot) * normal.X); + result.Y = vector.Y - ((2.0f * dot) * normal.Y); + + return result; + } + + /// + /// Returns a vector containing the smallest components of the specified vectors. + /// + /// The first source vector. + /// The second source vector. + /// A vector containing the smallest components of the source vectors. + public static LVector2 Minimize(LVector2 left, LVector2 right) + { + LVector2 vector; + vector.X = (left.X < right.X) ? left.X : right.X; + vector.Y = (left.Y < right.Y) ? left.Y : right.Y; + return vector; + } + /// + /// Returns a vector containing the largest components of the specified vectors. + /// + /// The first source vector. + /// The second source vector. + /// A vector containing the largest components of the source vectors. + public static LVector2 Maximize(LVector2 left, LVector2 right) + { + LVector2 vector; + vector.X = (left.X > right.X) ? left.X : right.X; + vector.Y = (left.Y > right.Y) ? left.Y : right.Y; + return vector; + } + + /// + /// Adds two vectors. + /// + /// The first vector to add. + /// The second vector to add. + /// The sum of the two vectors. + public static LVector2 operator +(LVector2 left, LVector2 right) => new LVector2(left.X + right.X, left.Y + right.Y); + + /// + /// Subtracts two vectors. + /// + /// The first vector to subtract. + /// The second vector to subtract. + /// The difference of the two vectors. + public static LVector2 operator -(LVector2 left, LVector2 right) => new LVector2(left.X - right.X, left.Y - right.Y); + + /// + /// Reverses the direction of a given vector. + /// + /// The vector to negate. + /// A vector facing in the opposite direction. + public static LVector2 operator -(LVector2 value) => new LVector2(-value.X, -value.Y); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector2 operator *(LVector2 vector, float scale) => new LVector2(vector.X * scale, vector.Y * scale); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector2 operator *(float scale, LVector2 vector) => new LVector2(vector.X * scale, vector.Y * scale); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector2 operator /(LVector2 vector, float scale) => new LVector2(vector.X / scale, vector.Y / scale); + + /// + /// Tests for equality between two objects. + /// + /// The first value to compare. + /// The second value to compare. + /// if has the same value as ; otherwise, . + public static bool operator ==(LVector2 left, LVector2 right) => Equals(left, right); + + /// + /// Tests for inequality between two objects. + /// + /// The first value to compare. + /// The second value to compare. + /// if has a different value than ; otherwise, . + public static bool operator !=(LVector2 left, LVector2 right) => !Equals(left, right); + + /// + /// Converts a Vector2 to a Vector3 implicitly. + /// + public static implicit operator LVector3(LVector2 vector) => new LVector3(vector.X, vector.Y, 0); + + /// + /// Converts the value of the object to its equivalent string representation. + /// + /// The string representation of the value of this instance. + public override string ToString() + { + return string.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1}", X, Y); + } + + /// + /// Converts the value of the object to its equivalent string representation. + /// + /// The format. + /// The string representation of the value of this instance. + public string ToString(string format) + { + if (format == null) + { + return ToString(); + } + + return string.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1}", X.ToString(format, CultureInfo.CurrentCulture), Y.ToString(format, CultureInfo.CurrentCulture)); + } + + /// + /// Returns the hash code for this instance. + /// + /// A 32-bit signed integer hash code. + public override int GetHashCode() + { + unchecked + { + return (X.GetHashCode() * 397) ^ Y.GetHashCode(); + } + } + + /// + /// Returns a value that indicates whether the current instance is equal to a specified object. + /// + /// Object to make the comparison with. + /// if the current instance is equal to the specified object; otherwise, . + public override bool Equals(object obj) + { + if (obj == null || obj.GetType() != GetType()) + { + return false; + } + + return Equals((LVector2)obj); + } + + /// + /// Returns a value that indicates whether the current instance is equal to the specified object. + /// + /// Object to make the comparison with. + /// if the current instance is equal to the specified object; otherwise. + public bool Equals(LVector2 other) => (X == other.X && Y == other.Y); + + + public static implicit operator LVector2(Vector2 v) => new(v.X, v.Y); + public static implicit operator Vector2(LVector2 v) => new(v.X, v.Y); + } +} diff --git a/Core/CompactVectors/LVector3.cs b/Core/CompactVectors/LVector3.cs new file mode 100644 index 00000000..2b5b954a --- /dev/null +++ b/Core/CompactVectors/LVector3.cs @@ -0,0 +1,731 @@ +// +// Copyright (C) 2007-2010 SlimDX Group +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and +// associated documentation files (the "Software"), to deal in the Software without restriction, +// including without limitation the rights to use, copy, modify, merge, publish, distribute, +// sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or +// substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT +// NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT +// OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +// +using GTA.Math; +using System.Globalization; +using System.Runtime.InteropServices; + +namespace RageCoop.Core +{ + + [Serializable] + public struct LVector3 : IEquatable + { + /// + /// Gets or sets the X component of the vector. + /// + /// The X component of the vector. + public float X; + + /// + /// Gets or sets the Y component of the vector. + /// + /// The Y component of the vector. + public float Y; + + /// + /// Gets or sets the Z component of the vector. + /// + /// The Z component of the vector. + public float Z; + + /// + /// Initializes a new instance of the class. + /// + /// Initial value for the X component of the vector. + /// Initial value for the Y component of the vector. + /// Initial value for the Z component of the vector. + public LVector3(float x, float y, float z) + { + X = x; + Y = y; + Z = z; + } + + internal LVector3(float[] values) : this(values[0], values[1], values[2]) + { + } + + /// + /// Returns this vector with a magnitude of 1. + /// + public LVector3 Normalized => Normalize(new LVector3(X, Y, Z)); + + /// + /// Returns a null vector. (0,0,0) + /// + public static LVector3 Zero => new(0.0f, 0.0f, 0.0f); + + /// + /// The X unit (1, 0, 0). + /// + public static LVector3 UnitX => new(1.0f, 0.0f, 0.0f); + + /// + /// The Y unit (0, 1, 0). + /// + public static LVector3 UnitY => new(0.0f, 1.0f, 0.0f); + + /// + /// The Z unit (0, 0, 1). + /// + public static LVector3 UnitZ => new(0.0f, 0.0f, 1.0f); + + /// + /// Returns the world Up vector. (0,0,1) + /// + public static LVector3 WorldUp => new(0.0f, 0.0f, 1.0f); + + /// + /// Returns the world Down vector. (0,0,-1) + /// + public static LVector3 WorldDown => new(0.0f, 0.0f, -1.0f); + + /// + /// Returns the world North vector. (0,1,0) + /// + public static LVector3 WorldNorth => new(0.0f, 1.0f, 0.0f); + + /// + /// Returns the world South vector. (0,-1,0) + /// + public static LVector3 WorldSouth => new(0.0f, -1.0f, 0.0f); + + /// + /// Returns the world East vector. (1,0,0) + /// + public static LVector3 WorldEast => new(1.0f, 0.0f, 0.0f); + + /// + /// Returns the world West vector. (-1,0,0) + /// + public static LVector3 WorldWest => new(-1.0f, 0.0f, 0.0f); + + /// + /// Returns the relative Right vector. (1,0,0) + /// + public static LVector3 RelativeRight => new(1.0f, 0.0f, 0.0f); + + /// + /// Returns the relative Left vector. (-1,0,0) + /// + public static LVector3 RelativeLeft => new(-1.0f, 0.0f, 0.0f); + + /// + /// Returns the relative Front vector. (0,1,0) + /// + public static LVector3 RelativeFront => new(0.0f, 1.0f, 0.0f); + + /// + /// Returns the relative Back vector. (0,-1,0) + /// + public static LVector3 RelativeBack => new(0.0f, -1.0f, 0.0f); + + /// + /// Returns the relative Top vector. (0,0,1) + /// + public static LVector3 RelativeTop => new(0.0f, 0.0f, 1.0f); + + /// + /// Returns the relative Bottom vector as used. (0,0,-1) + /// + public static LVector3 RelativeBottom => new(0.0f, 0.0f, -1.0f); + + /// + /// Gets or sets the component at the specified index. + /// + /// The value of the X, Y or Z component, depending on the index. + /// The index of the component to access. Use 0 for the X component, 1 for the Y component and 2 for the Z component. + /// The value of the component at the specified index. + /// Thrown when the is out of the range [0, 2]. + public float this[int index] + { + get + { + switch (index) + { + case 0: + return X; + case 1: + return Y; + case 2: + return Z; + } + + throw new ArgumentOutOfRangeException("index", "Indices for Vector3 run from 0 to 2, inclusive."); + } + + set + { + switch (index) + { + case 0: + X = value; + break; + case 1: + Y = value; + break; + case 2: + Z = value; + break; + default: + throw new ArgumentOutOfRangeException("index", + "Indices for Vector3 run from 0 to 2, inclusive."); + } + } + } + + /// + /// Calculates the length of the vector. + /// + /// The length of the vector. + public float Length() => (float)(System.Math.Sqrt((X * X) + (Y * Y) + (Z * Z))); + + /// + /// Calculates the squared length of the vector. + /// + /// The squared length of the vector. + public float LengthSquared() => (X * X) + (Y * Y) + (Z * Z); + + /// + /// Converts the vector into a unit vector. + /// + public void Normalize() + { + float length = Length(); + if (length == 0) + { + return; + } + + float num = 1 / length; + X *= num; + Y *= num; + Z *= num; + } + + /// + /// Calculates the distance between two vectors. + /// + /// The second vector to calculate the distance to. + /// The distance to the other vector. + public float DistanceTo(LVector3 position) => (position - this).Length(); + + /// + /// Calculates the squared distance between two vectors. + /// + /// The second vector to calculate the distance to. + /// The distance to the other vector. + public float DistanceToSquared(LVector3 position) => DistanceSquared(position, this); + + /// + /// Calculates the distance between two vectors, ignoring the Z-component. + /// + /// The second vector to calculate the distance to. + /// The distance to the other vector. + public float DistanceTo2D(LVector3 position) + { + var lhs = new LVector3(X, Y, 0.0f); + var rhs = new LVector3(position.X, position.Y, 0.0f); + + return Distance(lhs, rhs); + } + + /// + /// Calculates the squared distance between two vectors, ignoring the Z-component. + /// + /// The second vector to calculate the squared distance to. + /// The distance to the other vector. + public float DistanceToSquared2D(LVector3 position) + { + var lhs = new LVector3(X, Y, 0.0f); + var rhs = new LVector3(position.X, position.Y, 0.0f); + + return DistanceSquared(lhs, rhs); + } + + /// + /// Calculates the distance between two vectors. + /// + /// The first vector to calculate the distance to the second vector. + /// The second vector to calculate the distance to the first vector. + /// The distance between the two vectors. + public static float Distance(LVector3 position1, LVector3 position2) => (position1 - position2).Length(); + + /// + /// Calculates the squared distance between two vectors. + /// + /// The first vector to calculate the squared distance to the second vector. + /// The second vector to calculate the squared distance to the first vector. + /// The squared distance between the two vectors. + public static float DistanceSquared(LVector3 position1, LVector3 position2) => + (position1 - position2).LengthSquared(); + + /// + /// Calculates the distance between two vectors, ignoring the Z-component. + /// + /// The first vector to calculate the distance to the second vector. + /// The second vector to calculate the distance to the first vector. + /// The distance between the two vectors. + public static float Distance2D(LVector3 position1, LVector3 position2) + { + var pos1 = new LVector3(position1.X, position1.Y, 0); + var pos2 = new LVector3(position2.X, position2.Y, 0); + + return (pos1 - pos2).Length(); + } + + /// + /// Calculates the squared distance between two vectors, ignoring the Z-component. + /// + /// The first vector to calculate the squared distance to the second vector. + /// The second vector to calculate the squared distance to the first vector. + /// The squared distance between the two vectors. + public static float DistanceSquared2D(LVector3 position1, LVector3 position2) + { + var pos1 = new LVector3(position1.X, position1.Y, 0); + var pos2 = new LVector3(position2.X, position2.Y, 0); + + return (pos1 - pos2).LengthSquared(); + } + + /// + /// Returns the angle in degrees between from and to. + /// The angle returned is always the acute angle between the two vectors. + /// + public static float Angle(LVector3 from, LVector3 to) + { + double dot = Dot(from.Normalized, to.Normalized); + return (float)(System.Math.Acos((dot)) * (180.0 / System.Math.PI)); + } + + /// + /// Returns the signed angle in degrees between from and to. + /// + public static float SignedAngle(LVector3 from, LVector3 to, LVector3 planeNormal) + { + LVector3 perpVector = Cross(planeNormal, from); + + double angle = Angle(from, to); + double dot = Dot(perpVector, to); + if (dot < 0) + { + angle *= -1; + } + + return (float)angle; + } + + /// + /// Converts a vector to a heading. + /// + public float ToHeading() => (float)((System.Math.Atan2(X, -Y) + System.Math.PI) * (180.0 / System.Math.PI)); + + /// + /// Creates a random vector inside the circle around this position. + /// + public LVector3 Around(float distance) => this + (RandomXY() * distance); + + /// + /// Rounds each float inside the vector to a select amount of decimal places (2 by default). + /// + /// Number of decimal places to round to + /// The vector containing rounded values + public LVector3 Round(int decimalPlaces = 2) + { + return new LVector3((float)System.Math.Round(X, decimalPlaces), (float)System.Math.Round(Y, decimalPlaces), + (float)System.Math.Round(Z, decimalPlaces)); + } + + /// + /// Returns a new normalized vector with random X and Y components. + /// + public static LVector3 RandomXY() + { + LVector3 v = Zero; + double radian = CoreUtils.SafeRandom.NextDouble() * 2 * System.Math.PI; + + v.X = (float)(System.Math.Cos(radian)); + v.Y = (float)(System.Math.Sin(radian)); + v.Normalize(); + return v; + } + + /// + /// Returns a new normalized vector with random X, Y and Z components. + /// + public static LVector3 RandomXYZ() + { + LVector3 v = Zero; + double radian = CoreUtils.SafeRandom.NextDouble() * 2.0 * System.Math.PI; + double cosTheta = (CoreUtils.SafeRandom.NextDouble() * 2.0) - 1.0; + double theta = System.Math.Acos(cosTheta); + + v.X = (float)(System.Math.Sin(theta) * System.Math.Cos(radian)); + v.Y = (float)(System.Math.Sin(theta) * System.Math.Sin(radian)); + v.Z = (float)(System.Math.Cos(theta)); + v.Normalize(); + return v; + } + + /// + /// Adds two vectors. + /// + /// The first vector to add. + /// The second vector to add. + /// The sum of the two vectors. + public static LVector3 Add(LVector3 left, LVector3 right) => + new(left.X + right.X, left.Y + right.Y, left.Z + right.Z); + + /// + /// Subtracts two vectors. + /// + /// The first vector to subtract. + /// The second vector to subtract. + /// The difference of the two vectors. + public static LVector3 Subtract(LVector3 left, LVector3 right) => + new(left.X - right.X, left.Y - right.Y, left.Z - right.Z); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector3 Multiply(LVector3 value, float scale) => + new(value.X * scale, value.Y * scale, value.Z * scale); + + /// + /// Multiply a vector with another by performing component-wise multiplication. + /// + /// The first vector to multiply. + /// The second vector to multiply. + /// The multiplied vector. + public static LVector3 Multiply(LVector3 left, LVector3 right) => + new(left.X * right.X, left.Y * right.Y, left.Z * right.Z); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector3 Divide(LVector3 value, float scale) => + new(value.X / scale, value.Y / scale, value.Z / scale); + + /// + /// Reverses the direction of a given vector. + /// + /// The vector to negate. + /// A vector facing in the opposite direction. + public static LVector3 Negate(LVector3 value) => new(-value.X, -value.Y, -value.Z); + + /// + /// Restricts a value to be within a specified range. + /// + /// The value to clamp. + /// The minimum value. + /// The maximum value. + /// The clamped value. + public static LVector3 Clamp(LVector3 value, LVector3 min, LVector3 max) + { + float x = value.X; + x = (x > max.X) ? max.X : x; + x = (x < min.X) ? min.X : x; + + float y = value.Y; + y = (y > max.Y) ? max.Y : y; + y = (y < min.Y) ? min.Y : y; + + float z = value.Z; + z = (z > max.Z) ? max.Z : z; + z = (z < min.Z) ? min.Z : z; + + return new LVector3(x, y, z); + } + + /// + /// Performs a linear interpolation between two vectors. + /// + /// Start vector. + /// End vector. + /// Value between 0 and 1 indicating the weight of . + /// The linear interpolation of the two vectors. + /// + /// This method performs the linear interpolation based on the following formula. + /// start + (end - start) * amount + /// Passing a value of 0 will cause to be returned; a value of 1 will cause to be returned. + /// + public static LVector3 Lerp(LVector3 start, LVector3 end, float amount) + { + LVector3 vector = Zero; + + vector.X = start.X + ((end.X - start.X) * amount); + vector.Y = start.Y + ((end.Y - start.Y) * amount); + vector.Z = start.Z + ((end.Z - start.Z) * amount); + + return vector; + } + + /// + /// Converts the vector into a unit vector. + /// + /// The vector to normalize. + /// The normalized vector. + public static LVector3 Normalize(LVector3 vector) + { + vector.Normalize(); + return vector; + } + + /// + /// Calculates the dot product of two vectors. + /// + /// First source vector. + /// Second source vector. + /// The dot product of the two vectors. + public static float Dot(LVector3 left, LVector3 right) => + (left.X * right.X + left.Y * right.Y + left.Z * right.Z); + + /// + /// Calculates the cross product of two vectors. + /// + /// First source vector. + /// Second source vector. + /// The cross product of the two vectors. + public static LVector3 Cross(LVector3 left, LVector3 right) + { + LVector3 result = Zero; + result.X = left.Y * right.Z - left.Z * right.Y; + result.Y = left.Z * right.X - left.X * right.Z; + result.Z = left.X * right.Y - left.Y * right.X; + return result; + } + + /// + /// Projects a vector onto another vector. + /// + /// The vector to project. + /// Vector to project onto, does not assume it is normalized. + /// The projected vector. + public static LVector3 Project(LVector3 vector, LVector3 onNormal) => + onNormal * Dot(vector, onNormal) / Dot(onNormal, onNormal); + + /// + /// Projects a vector onto a plane defined by a normal orthogonal to the plane. + /// + /// The vector to project. + /// Normal of the plane, does not assume it is normalized. + /// The Projection of vector onto plane. + public static LVector3 ProjectOnPlane(LVector3 vector, LVector3 planeNormal) => + (vector - Project(vector, planeNormal)); + + /// + /// Returns the reflection of a vector off a surface that has the specified normal. + /// + /// The vector to project onto the plane. + /// Normal of the surface. + /// The reflected vector. + /// Reflect only gives the direction of a reflection off a surface, it does not determine + /// whether the original vector was close enough to the surface to hit it. + public static LVector3 Reflect(LVector3 vector, LVector3 normal) + { + LVector3 result = Zero; + float dot = ((vector.X * normal.X) + (vector.Y * normal.Y)) + (vector.Z * normal.Z); + + result.X = vector.X - ((2.0f * dot) * normal.X); + result.Y = vector.Y - ((2.0f * dot) * normal.Y); + result.Z = vector.Z - ((2.0f * dot) * normal.Z); + + return result; + } + + /// + /// Returns a vector containing the smallest components of the specified vectors. + /// + /// The first source vector. + /// The second source vector. + /// A vector containing the smallest components of the source vectors. + public static LVector3 Minimize(LVector3 left, LVector3 right) + { + LVector3 vector = Zero; + vector.X = (left.X < right.X) ? left.X : right.X; + vector.Y = (left.Y < right.Y) ? left.Y : right.Y; + vector.Z = (left.Z < right.Z) ? left.Z : right.Z; + return vector; + } + + /// + /// Returns a vector containing the largest components of the specified vectors. + /// + /// The first source vector. + /// The second source vector. + /// A vector containing the largest components of the source vectors. + public static LVector3 Maximize(LVector3 left, LVector3 right) + { + LVector3 vector = Zero; + vector.X = (left.X > right.X) ? left.X : right.X; + vector.Y = (left.Y > right.Y) ? left.Y : right.Y; + vector.Z = (left.Z > right.Z) ? left.Z : right.Z; + return vector; + } + + /// + /// Adds two vectors. + /// + /// The first vector to add. + /// The second vector to add. + /// The sum of the two vectors. + public static LVector3 operator +(LVector3 left, LVector3 right) => + new(left.X + right.X, left.Y + right.Y, left.Z + right.Z); + + /// + /// Subtracts two vectors. + /// + /// The first vector to subtract. + /// The second vector to subtract. + /// The difference of the two vectors. + public static LVector3 operator -(LVector3 left, LVector3 right) => + new(left.X - right.X, left.Y - right.Y, left.Z - right.Z); + + /// + /// Reverses the direction of a given vector. + /// + /// The vector to negate. + /// A vector facing in the opposite direction. + public static LVector3 operator -(LVector3 vector) => new(-vector.X, -vector.Y, -vector.Z); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector3 operator *(LVector3 vector, float scale) => + new(vector.X * scale, vector.Y * scale, vector.Z * scale); + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector3 operator *(float scale, LVector3 vector) => vector * scale; + + /// + /// Scales a vector by the given value. + /// + /// The vector to scale. + /// The amount by which to scale the vector. + /// The scaled vector. + public static LVector3 operator /(LVector3 vector, float scale) + { + float invScale = 1.0f / scale; + return new LVector3(vector.X * invScale, vector.Y * invScale, vector.Z * invScale); + } + + /// + /// Tests for equality between two objects. + /// + /// The first value to compare. + /// The second value to compare. + /// if has the same value as ; otherwise, . + public static bool operator ==(LVector3 left, LVector3 right) => Equals(left, right); + + /// + /// Tests for inequality between two objects. + /// + /// The first value to compare. + /// The second value to compare. + /// if has a different value than ; otherwise, . + public static bool operator !=(LVector3 left, LVector3 right) => !Equals(left, right); + + /// + /// Converts a Vector3 to a Vector2 implicitly. + /// + public static implicit operator LVector2(LVector3 vector) => new(vector.X, vector.Y); + + /// + /// Converts the matrix to an array of floats. + /// + public float[] ToArray() => new[] { X, Y, Z }; + + /// + /// Converts the value of the object to its equivalent string representation. + /// + /// The string representation of the value of this instance. + public override string ToString() => string.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2}", X, Y, Z); + + /// + /// Converts the value of the object to its equivalent string representation. + /// + /// The number format. + /// The string representation of the value of this instance. + public string ToString(string format) + { + if (format == null) + { + return ToString(); + } + + return string.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2}", + X.ToString(format, CultureInfo.CurrentCulture), + Y.ToString(format, CultureInfo.CurrentCulture), Z.ToString(format, CultureInfo.CurrentCulture)); + } + + /// + /// Returns the hash code for this instance. + /// + /// A 32-bit signed integer hash code. + public override int GetHashCode() + { + unchecked + { + var hashCode = X.GetHashCode(); + hashCode = (hashCode * 397) ^ Y.GetHashCode(); + hashCode = (hashCode * 397) ^ Z.GetHashCode(); + return hashCode; + } + } + + /// + /// Returns a value that indicates whether the current instance is equal to a specified object. + /// + /// Object to make the comparison with. + /// if the current instance is equal to the specified object; otherwise. + public override bool Equals(object obj) + { + if (obj == null || obj.GetType() != GetType()) + { + return false; + } + + return Equals((LVector3)obj); + } + + /// + /// Returns a value that indicates whether the current instance is equal to the specified object. + /// + /// Object to make the comparison with. + /// if the current instance is equal to the specified object; otherwise. + public bool Equals(LVector3 other) => (X == other.X && Y == other.Y && Z == other.Z); + + public static implicit operator LVector3(Vector3 v) => new(v.X, v.Y, v.Z); + public static implicit operator Vector3(LVector3 v) => new(v.X, v.Y, v.Z); + } +} \ No newline at end of file