401 lines
13 KiB
C#
401 lines
13 KiB
C#
using System;
|
|
using System.Numerics;
|
|
|
|
namespace LinearAlgebra
|
|
{
|
|
|
|
public class Vector2Of<T> where T : IComparable<T>
|
|
{
|
|
public T x;
|
|
public T y;
|
|
|
|
public Vector2Of(T x, T y)
|
|
{
|
|
this.x = x;
|
|
this.y = y;
|
|
}
|
|
}
|
|
|
|
public class Vector2Int : Vector2Of<int>
|
|
{
|
|
public Vector2Int(int x, int y) : base(x, y) { }
|
|
|
|
public static Vector2Int operator -(Vector2Int v1, Vector2Int v2)
|
|
{
|
|
return new Vector2Int(v1.x - v2.x, v1.y - v2.y);
|
|
}
|
|
|
|
public float magnitude
|
|
{
|
|
get
|
|
{
|
|
return (float)Math.Sqrt(this.x * this.x + this.y * this.y);
|
|
}
|
|
}
|
|
|
|
public static float Distance(Vector2Int v1, Vector2Int v2)
|
|
{
|
|
return (v1 - v2).magnitude;
|
|
}
|
|
}
|
|
|
|
public class Vector2Float : Vector2Of<float>
|
|
{
|
|
public Vector2Float(float x, float y) : base(x, y) { }
|
|
|
|
public static Vector2Float operator -(Vector2Float v1, Vector2Float v2)
|
|
{
|
|
return new Vector2Float(v1.x - v2.x, v1.y - v2.y);
|
|
}
|
|
|
|
public float magnitude
|
|
{
|
|
get
|
|
{
|
|
return (float)Math.Sqrt(this.x * this.x + this.y * this.y);
|
|
}
|
|
}
|
|
|
|
public static float Distance(Vector2Float v1, Vector2Float v2)
|
|
{
|
|
return (v1 - v2).magnitude;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 2-dimensional vectors
|
|
/// </summary>
|
|
public struct Vector2 : IEquatable<Vector2>
|
|
{
|
|
|
|
/// <summary>
|
|
/// The right axis of the vector
|
|
/// </summary>
|
|
public float x; // left/right
|
|
/// <summary>
|
|
/// The upward/forward axis of the vector
|
|
/// </summary>
|
|
public float y; // forward/backward
|
|
// directions are to be inline with Vector3 as much as possible...
|
|
|
|
/// <summary>
|
|
/// Create a new 2-dimensional vector
|
|
/// </summary>
|
|
/// <param name="x">x axis value</param>
|
|
/// <param name="y">y axis value</param>
|
|
public Vector2(float x, float y)
|
|
{
|
|
this.x = x;
|
|
this.y = y;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A vector with zero for all axis
|
|
/// </summary>
|
|
public static readonly Vector2 zero = new Vector2(0, 0);
|
|
/// <summary>
|
|
/// A vector with values (1, 1)
|
|
/// </summary>
|
|
public static readonly Vector2 one = new Vector2(1, 1);
|
|
/// <summary>
|
|
/// A vector with values (0, 1)
|
|
/// </summary>
|
|
public static readonly Vector2 up = new Vector2(0, 1);
|
|
/// <summary>
|
|
/// A vector with values (0, -1)
|
|
/// </summary>
|
|
public static readonly Vector2 down = new Vector2(0, -1);
|
|
/// <summary>
|
|
/// A vector with values (0, 1)
|
|
/// </summary>
|
|
public static readonly Vector2 forward = new Vector2(0, 1);
|
|
/// <summary>
|
|
/// A vector with values (0, -1)
|
|
/// </summary>
|
|
public static readonly Vector2 back = new Vector2(0, -1);
|
|
/// <summary>
|
|
/// A vector3 with values (-1, 0)
|
|
/// </summary>
|
|
public static readonly Vector2 left = new Vector2(-1, 0);
|
|
/// <summary>
|
|
/// A vector with values (1, 0)
|
|
/// </summary>
|
|
public static readonly Vector2 right = new Vector2(1, 0);
|
|
|
|
/// <summary>
|
|
/// The squared length of this vector
|
|
/// </summary>
|
|
/// <returns>The squared length</returns>
|
|
/// The squared length is computationally simpler than the real length.
|
|
/// Think of Pythagoras A^2 + B^2 = C^2.
|
|
/// This leaves out the calculation of the squared root of C.
|
|
public float sqrMagnitude
|
|
{
|
|
get
|
|
{
|
|
float d = x * x + y * y;
|
|
return d;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The length of this vector
|
|
/// </summary>
|
|
/// <returns>The length of this vector</returns>
|
|
public float magnitude
|
|
{
|
|
get
|
|
{
|
|
float d = (float)Math.Sqrt(x * x + y * y);
|
|
return d;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert the vector to a length of a 1
|
|
/// </summary>
|
|
/// <returns>The vector with length 1</returns>
|
|
public Vector2 normalized
|
|
{
|
|
get
|
|
{
|
|
float l = magnitude;
|
|
Vector2 v = zero;
|
|
if (l > Float.epsilon)
|
|
v = this / l;
|
|
return v;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Add two vectors
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns>The result of adding the two vectors</returns>
|
|
public static Vector2 operator +(Vector2 v1, Vector2 v2)
|
|
{
|
|
Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Subtract two vectors
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns>The result of adding the two vectors</returns>
|
|
public static Vector2 operator -(Vector2 v1, Vector2 v2)
|
|
{
|
|
Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Negate the vector
|
|
/// </summary>
|
|
/// <param name="v1">The vector to negate</param>
|
|
/// <returns>The negated vector</returns>
|
|
/// This will result in a vector pointing in the opposite direction
|
|
public static Vector2 operator -(Vector2 v1)
|
|
{
|
|
Vector2 v = new Vector2(-v1.x, -v1.y);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scale a vector uniformly up
|
|
/// </summary>
|
|
/// <param name="v1">The vector to scale</param>
|
|
/// <param name="f">The scaling factor</param>
|
|
/// <returns>The scaled vector</returns>
|
|
/// Each component of the vector will be multipled with the same factor.
|
|
public static Vector2 operator *(Vector2 v1, float f)
|
|
{
|
|
Vector2 v = new Vector2(v1.x * f, v1.y * f);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scale a vector uniformly up
|
|
/// </summary>
|
|
/// <param name="f">The scaling factor</param>
|
|
/// <param name="v1">The vector to scale</param>
|
|
/// <returns>The scaled vector</returns>
|
|
/// Each component of the vector will be multipled with the same factor.
|
|
public static Vector2 operator *(float f, Vector2 v1)
|
|
{
|
|
Vector2 v = new Vector2(f * v1.x, f * v1.y);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scale a vector uniformly down
|
|
/// </summary>
|
|
/// <param name="v1">The vector to scale</param>
|
|
/// <param name="f">The scaling factor</param>
|
|
/// <returns>The scaled vector</returns>
|
|
/// Each component of the vector will be devided by the same factor.
|
|
public static Vector2 operator /(Vector2 v1, float f)
|
|
{
|
|
Vector2 v = new Vector2(v1.x / f, v1.y / f);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tests if the vector has equal values as the given vector
|
|
/// </summary>
|
|
/// <param name="v1">The vector to compare to</param>
|
|
/// <returns><em>true</em> if the vector values are equal</returns>
|
|
public bool Equals(Vector2 v1) => x == v1.x && y == v1.y;
|
|
|
|
/// <summary>
|
|
/// Tests if the vector is equal to the given object
|
|
/// </summary>
|
|
/// <param name="obj">The object to compare to</param>
|
|
/// <returns><em>false</em> when the object is not a Vector2 or does not have equal values</returns>
|
|
public override bool Equals(object obj)
|
|
{
|
|
if (!(obj is Vector2 v))
|
|
return false;
|
|
|
|
return (x == v.x && y == v.y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tests if the two vectors have equal values
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns><em>true</em>when the vectors have equal values</returns>
|
|
/// Note that this uses a Float equality check which cannot be not exact in all cases.
|
|
/// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
|
|
/// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
|
|
public static bool operator ==(Vector2 v1, Vector2 v2)
|
|
{
|
|
return (v1.x == v2.x && v1.y == v2.y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tests if two vectors have different values
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns><em>true</em>when the vectors have different values</returns>
|
|
/// Note that this uses a Float equality check which cannot be not exact in all case.
|
|
/// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
|
|
/// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
|
|
public static bool operator !=(Vector2 v1, Vector2 v2)
|
|
{
|
|
return (v1.x != v2.x || v1.y != v2.y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get an hash code for the vector
|
|
/// </summary>
|
|
/// <returns>The hash code</returns>
|
|
public override int GetHashCode()
|
|
{
|
|
return (x, y).GetHashCode();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the distance between two vectors
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns>The distance between the two vectors</returns>
|
|
public static float Distance(Vector2 v1, Vector2 v2)
|
|
{
|
|
float x = v1.x - v2.x;
|
|
float y = v1.y - v2.y;
|
|
float d = (float)Math.Sqrt(x * x + y * y);
|
|
return d;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The dot product of two vectors
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns>The dot product of the two vectors</returns>
|
|
public static float Dot(Vector2 v1, Vector2 v2)
|
|
{
|
|
return v1.x * v2.x + v1.y * v2.y;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lerp between two vectors
|
|
/// </summary>
|
|
/// <param name="v1">The from vector</param>
|
|
/// <param name="v2">The to vector</param>
|
|
/// <param name="f">The interpolation distance [0..1]</param>
|
|
/// <returns>The lerped vector</returns>
|
|
/// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1
|
|
/// matches the *v2* vector Value -1 is *v1* vector minus the difference
|
|
/// between *v1* and *v2* etc.
|
|
public static Vector2 Lerp(Vector2 v1, Vector2 v2, float f)
|
|
{
|
|
Vector2 v = v1 + (v2 - v1) * f;
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the signed angle between two vectors.
|
|
/// </summary>
|
|
/// <param name="from">The starting vector</param>
|
|
/// <param name="to">The ending vector</param>
|
|
/// <param name="axis">The axis to rotate around</param>
|
|
/// <returns>The signed angle in degrees</returns>
|
|
public static float SignedAngle(Vector2 from, Vector2 to)
|
|
{
|
|
//float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y);
|
|
//return Vector2.Angle(v1, v2) * sign;
|
|
|
|
float sqrMagFrom = from.sqrMagnitude;
|
|
float sqrMagTo = to.sqrMagnitude;
|
|
|
|
if (sqrMagFrom == 0 || sqrMagTo == 0)
|
|
return 0;
|
|
//if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
|
|
// return nanf("");
|
|
|
|
float angleFrom = (float)Math.Atan2(from.y, from.x);
|
|
float angleTo = (float)Math.Atan2(to.y, to.x);
|
|
return (angleTo - angleFrom) * Angle.Rad2Deg;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rotates the vector with the given angle
|
|
/// </summary>
|
|
/// <param name="v1">The vector to rotate</param>
|
|
/// <param name="angle">The angle in degrees</param>
|
|
/// <returns></returns>
|
|
public static Vector2 Rotate(Vector2 v1, float angle)
|
|
{
|
|
float sin = (float)Math.Sin(angle * Angle.Deg2Rad);
|
|
float cos = (float)Math.Cos(angle * Angle.Deg2Rad);
|
|
|
|
float tx = v1.x;
|
|
float ty = v1.y;
|
|
Vector2 v = new Vector2()
|
|
{
|
|
x = (cos * tx) - (sin * ty),
|
|
y = (sin * tx) + (cos * ty)
|
|
};
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Map interval of angles between vectors [0..Pi] to interval [0..1]
|
|
/// </summary>
|
|
/// <param name="v1">The first vector</param>
|
|
/// <param name="v2">The second vector</param>
|
|
/// <returns>The resulting factor in interval [0..1]</returns>
|
|
/// Vectors a and b must be normalized
|
|
public static float ToFactor(Vector2 v1, Vector2 v2)
|
|
{
|
|
return (1 - Vector2.Dot(v1, v2)) / 2;
|
|
}
|
|
}
|
|
} |