273 lines
11 KiB
C#
273 lines
11 KiB
C#
//#if !UNITY_5_3_OR_NEWER
|
|
using System;
|
|
|
|
namespace LinearAlgebra {
|
|
|
|
/// <summary>
|
|
/// 3-dimensional vectors
|
|
/// </summary>
|
|
/// This uses the right-handed coordinate system.
|
|
/// <remarks>
|
|
/// Create a new 3-dimensional vector
|
|
/// </remarks>
|
|
/// <param name="horizontal">x axis value</param>
|
|
/// <param name="vertical">y axis value</param>
|
|
/// <param name="depth">z axis value</param>
|
|
public struct Vector3Int {
|
|
|
|
/// <summary>
|
|
/// The right axis of the vector
|
|
/// </summary>
|
|
public int horizontal; //> left/right
|
|
/// <summary>
|
|
/// The upward axis of the vector
|
|
/// </summary>
|
|
public int vertical; //> up/down
|
|
/// <summary>
|
|
/// The forward axis of the vector
|
|
/// </summary>
|
|
public int depth; //> forward/backward
|
|
|
|
public Vector3Int(int horizontal, int vertical, int depth) {
|
|
this.horizontal = horizontal;
|
|
this.vertical = vertical;
|
|
this.depth = depth;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A vector with zero for all axis
|
|
/// </summary>
|
|
public static readonly Vector3Int zero = new(0, 0, 0);
|
|
/// <summary>
|
|
/// A vector with one for all axis
|
|
/// </summary>
|
|
public static readonly Vector3Int one = new(1, 1, 1);
|
|
/// <summary>
|
|
/// A Vector3Int with values (-1, 0, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Int left = new(-1, 0, 0);
|
|
/// <summary>
|
|
/// A vector with values (1, 0, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Int right = new(1, 0, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, -1, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Int down = new(0, -1, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, 1, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Int up = new(0, 1, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, 0, -1)
|
|
/// </summary>
|
|
public static readonly Vector3Int back = new(0, -1, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, 0, 1)
|
|
/// </summary>
|
|
public static readonly Vector3Int forward = new(0, 1, 0);
|
|
|
|
/// @brief The vector length
|
|
/// @return The vector length
|
|
public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical + depth * depth);
|
|
/// <summary>
|
|
/// The vector length
|
|
/// </summary>
|
|
/// <param name="v">The vector for which you need the length</param>
|
|
/// <returns>The vector length</returns>
|
|
public static float MagnitudeOf(Vector3Int v) {
|
|
return v.magnitude;
|
|
}
|
|
|
|
/// @brief The squared vector length
|
|
/// @return The squared vector length
|
|
/// @remark 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 readonly float sqrMagnitude => (horizontal * horizontal + vertical * vertical + depth * depth);
|
|
|
|
/// <summary>
|
|
/// The squared vector length
|
|
/// </summary>
|
|
/// <param name="v">The vector for which you need the squared length</param>
|
|
/// <returns>The squared vector length</returns>
|
|
/// <remarks>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.</remarks>
|
|
public static float SqrMagnitudeOf(Vector3Int v) {
|
|
return v.sqrMagnitude;
|
|
}
|
|
|
|
/// @brief Convert the vector to a length of 1
|
|
/// @return The vector normalized to a length of 1
|
|
public readonly Vector3Float normalized {
|
|
get {
|
|
float l = magnitude;
|
|
Vector3Float v = Vector3Float.zero;
|
|
if (l > Float.epsilon)
|
|
v = new Vector3Float(this) / l;
|
|
return v;
|
|
}
|
|
}
|
|
/// @brief Convert the vector to a length of 1
|
|
/// @param v The vector to convert
|
|
/// @return The vector normalized to a length of 1
|
|
public static Vector3Float Normalize(Vector3Int v) {
|
|
float num = v.magnitude;
|
|
Vector3Float result = Vector3Float.zero;
|
|
if (num > Float.epsilon)
|
|
result = new Vector3Float(v) / num;
|
|
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Negate te vector such that it points in the opposite direction
|
|
/// </summary>
|
|
/// <param name="v1"></param>
|
|
/// <returns>The negated vector</returns>
|
|
public static Vector3Int operator -(Vector3Int v1) {
|
|
Vector3Int v = new(-v1.horizontal, -v1.vertical, -v1.depth);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Subtract two vectors
|
|
/// </summary>
|
|
/// <param name="v1"></param>
|
|
/// <param name="v2"></param>
|
|
/// <returns>The result of the subtraction</returns>
|
|
public static Vector3Int operator -(Vector3Int v1, Vector3Int v2) {
|
|
Vector3Int v = new(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical, v1.depth - v2.depth);
|
|
return v;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Add two vectors
|
|
/// </summary>
|
|
/// <param name="v1"></param>
|
|
/// <param name="v2"></param>
|
|
/// <returns>The result of the addition</returns>
|
|
public static Vector3Int operator +(Vector3Int v1, Vector3Int v2) {
|
|
Vector3Int v = new(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical, v1.depth + v2.depth);
|
|
return v;
|
|
}
|
|
|
|
/// @brief Scale the vector using another vector
|
|
/// @param v1 The vector to scale
|
|
/// @param v2 A vector with the scaling factors
|
|
/// @return The scaled vector
|
|
/// @remark Each component of the vector v1 will be multiplied with the
|
|
/// matching component from the scaling vector v2.
|
|
public static Vector3Int Scale(Vector3Int v1, Vector3Int v2) {
|
|
return new Vector3Int(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical, v1.depth * v2.depth);
|
|
}
|
|
|
|
|
|
public static Vector3Int operator *(Vector3Int v1, int d) {
|
|
Vector3Int v = new(v1.horizontal * d, v1.vertical * d, v1.depth * d);
|
|
return v;
|
|
}
|
|
|
|
public static Vector3Int operator *(int d, Vector3Int v1) {
|
|
Vector3Int v = new(d * v1.horizontal, d * v1.vertical, d * v1.depth);
|
|
return v;
|
|
}
|
|
|
|
public static Vector3Int operator /(Vector3Int v1, int d) {
|
|
Vector3Int v = new(v1.horizontal / d, v1.vertical / d, v1.depth / d);
|
|
return v;
|
|
}
|
|
|
|
public bool Equals(Vector3Int v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
|
|
|
|
public override bool Equals(object obj) {
|
|
if (!(obj is Vector3Int v))
|
|
return false;
|
|
|
|
return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
|
|
}
|
|
|
|
public static bool operator ==(Vector3Int v1, Vector3Int v2) {
|
|
return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth);
|
|
}
|
|
|
|
public static bool operator !=(Vector3Int v1, Vector3Int v2) {
|
|
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth);
|
|
}
|
|
|
|
public override int GetHashCode() {
|
|
return (horizontal, vertical, depth).GetHashCode();
|
|
}
|
|
|
|
/// @brief The distance between two vectors
|
|
/// @param v1 The first vector
|
|
/// @param v2 The second vector
|
|
/// @return The distance between the two vectors
|
|
public static float Distance(Vector3Int v1, Vector3Int v2) {
|
|
return (v2 - v1).magnitude;
|
|
}
|
|
|
|
/// @brief The dot product of two vectors
|
|
/// @param v1 The first vector
|
|
/// @param v2 The second vector
|
|
/// @return The dot product of the two vectors
|
|
public static float Dot(Vector3Int v1, Vector3Int v2) {
|
|
return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + v1.depth * v2.depth;
|
|
}
|
|
|
|
/// @brief The cross product of two vectors
|
|
/// @param v1 The first vector
|
|
/// @param v2 The second vector
|
|
/// @return The cross product of the two vectors
|
|
public static Vector3Int Cross(Vector3Int v1, Vector3Int v2) {
|
|
return new Vector3Int(v1.vertical * v2.depth - v1.depth * v2.vertical, v1.depth * v2.horizontal - v1.horizontal * v2.depth,
|
|
v1.horizontal * v2.vertical - v1.vertical * v2.horizontal);
|
|
|
|
}
|
|
|
|
/// @brief The angle between two vectors
|
|
/// @param v1 The first vector
|
|
/// @param v2 The second vector
|
|
/// @return The angle between the two vectors
|
|
/// @remark This reterns an unsigned angle which is the shortest distance
|
|
/// between the two vectors. Use Vector3::SignedAngle if a signed angle is
|
|
/// needed.
|
|
public static AngleFloat UnsignedAngle(Vector3Int v1, Vector3Int v2) {
|
|
float denominator = MathF.Sqrt(v1.sqrMagnitude * v2.sqrMagnitude);
|
|
if (denominator < Float.epsilon)
|
|
return AngleFloat.zero;
|
|
|
|
float dot = Dot(v1, v2);
|
|
float fraction = dot / denominator;
|
|
if (float.IsNaN(fraction))
|
|
return AngleFloat.Degrees(
|
|
fraction); // short cut to returning NaN universally
|
|
|
|
float cdot = Float.Clamp(fraction, -1.0f, 1.0f);
|
|
float r = MathF.Acos(cdot);
|
|
return AngleFloat.Radians(r);
|
|
}
|
|
/// @brief The signed angle between two vectors
|
|
/// @param v1 The starting vector
|
|
/// @param v2 The ending vector
|
|
/// @param axis The axis to rotate around
|
|
/// @return The signed angle between the two vectors
|
|
public static AngleFloat SignedAngle(Vector3Int v1, Vector3Int v2,
|
|
Vector3Int axis) {
|
|
// angle in [0,180]
|
|
AngleFloat angle = UnsignedAngle(v1, v2);
|
|
|
|
Vector3Int cross = Cross(v1, v2);
|
|
float b = Dot(axis, cross);
|
|
float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
|
|
|
|
// angle in [-179,180]
|
|
AngleFloat signed_angle = angle * signd;
|
|
|
|
return signed_angle;
|
|
}
|
|
|
|
}
|
|
}
|
|
//#endif |