402 lines
16 KiB
C#
402 lines
16 KiB
C#
//#if !UNITY_5_3_OR_NEWER
|
|
using System;
|
|
|
|
namespace LinearAlgebra {
|
|
/*
|
|
public struct Vector3Float {
|
|
public float horizontal;
|
|
public float vertical;
|
|
public float depth;
|
|
|
|
public Vector3Float(float horizontal, float vertical, float depth) {
|
|
this.horizontal = horizontal;
|
|
this.vertical = vertical;
|
|
this.depth = depth;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A vector with zero for all axis
|
|
/// </summary>
|
|
public static readonly Vector3Float zero = new(0, 0, 0);
|
|
|
|
public readonly float magnitude {
|
|
get => (float)Math.Sqrt(this.horizontal * this.horizontal + this.vertical * this.vertical + this.depth * this.depth);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert the vector to a length of a 1
|
|
/// </summary>
|
|
/// <returns>The vector with length 1</returns>
|
|
public readonly Vector3Float normalized {
|
|
get {
|
|
float l = magnitude;
|
|
Vector3Float v = zero;
|
|
if (l > Float.epsilon)
|
|
v = this / l;
|
|
return v;
|
|
}
|
|
}
|
|
|
|
|
|
public static Vector3Float operator *(Vector3Float v, float f) {
|
|
Vector3Float r = new(v.horizontal * f, v.vertical * f, v.depth * f);
|
|
return r;
|
|
}
|
|
public static Vector3Float operator /(Vector3Float v, float f) {
|
|
Vector3Float r = new(v.horizontal / f, v.vertical / f, v.depth / f);
|
|
return r;
|
|
}
|
|
|
|
public static float Dot(Vector3Float v1, Vector3Float v2) {
|
|
return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical +
|
|
v1.depth * v2.depth;
|
|
}
|
|
|
|
const float epsilon = 1E-05f;
|
|
public static Vector3Float Project(Vector3Float v, Vector3Float n) {
|
|
float sqrMagnitude = Dot(n, n);
|
|
if (sqrMagnitude < epsilon)
|
|
return zero;
|
|
else {
|
|
float dot = Dot(v, n);
|
|
Vector3Float r = n * dot;
|
|
r /= sqrMagnitude;
|
|
return r;
|
|
}
|
|
|
|
}
|
|
}
|
|
*/
|
|
|
|
/// <summary>
|
|
/// 3-dimensional vectors
|
|
/// </summary>
|
|
/// This uses the right-handed coordinate system.
|
|
public struct Vector3Float {
|
|
|
|
/// <summary>
|
|
/// The right axis of the vector
|
|
/// </summary>
|
|
public float horizontal; //> left/right
|
|
/// <summary>
|
|
/// The upward axis of the vector
|
|
/// </summary>
|
|
public float vertical; //> up/down
|
|
/// <summary>
|
|
/// The forward axis of the vector
|
|
/// </summary>
|
|
public float depth; //> forward/backward
|
|
|
|
/// <summary>
|
|
/// Create a new 3-dimensional vector
|
|
/// </summary>
|
|
/// <param name="horizontal">x axis value</param>
|
|
/// <param name="vertical">y axis value</param>
|
|
/// <param name="depth">z axis value</param>
|
|
public Vector3Float(float horizontal, float vertical, float depth) {
|
|
this.horizontal = horizontal;
|
|
this.vertical = vertical;
|
|
this.depth = depth;
|
|
}
|
|
|
|
public Vector3Float(Vector3Int v) {
|
|
this.horizontal = v.horizontal;
|
|
this.vertical = v.vertical;
|
|
this.depth = v.depth;
|
|
}
|
|
|
|
public static Vector3Float FromSpherical(Spherical s) {
|
|
float verticalRad = (AngleFloat.deg90 - s.direction.vertical).inRadians;
|
|
float horizontalRad = s.direction.horizontal.inRadians;
|
|
float cosVertical = MathF.Cos(verticalRad);
|
|
float sinVertical = MathF.Sin(verticalRad);
|
|
float cosHorizontal = MathF.Cos(horizontalRad);
|
|
float sinHorizontal = MathF.Sin(horizontalRad);
|
|
|
|
float horizontal = s.distance * sinVertical * sinHorizontal;
|
|
float vertical = s.distance * cosVertical;
|
|
float depth = s.distance * sinVertical * cosHorizontal;
|
|
return new Vector3Float(horizontal, vertical, depth);
|
|
}
|
|
|
|
public override string ToString() {
|
|
return $"({this.horizontal}, {this.vertical}, {this.depth})";
|
|
}
|
|
|
|
/// <summary>
|
|
/// A vector with zero for all axis
|
|
/// </summary>
|
|
public static readonly Vector3Float zero = new Vector3Float(0, 0, 0);
|
|
/// <summary>
|
|
/// A vector with one for all axis
|
|
/// </summary>
|
|
public static readonly Vector3Float one = new Vector3Float(1, 1, 1);
|
|
/// <summary>
|
|
/// A Vector3Float with values (-1, 0, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Float left = new Vector3Float(-1, 0, 0);
|
|
/// <summary>
|
|
/// A vector with values (1, 0, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Float right = new Vector3Float(1, 0, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, -1, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Float down = new Vector3Float(0, -1, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, 1, 0)
|
|
/// </summary>
|
|
public static readonly Vector3Float up = new Vector3Float(0, 1, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, 0, -1)
|
|
/// </summary>
|
|
public static readonly Vector3Float back = new Vector3Float(0, -1, 0);
|
|
/// <summary>
|
|
/// A vector with values (0, 0, 1)
|
|
/// </summary>
|
|
public static readonly Vector3Float forward = new Vector3Float(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(Vector3Float 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(Vector3Float 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 = zero;
|
|
if (l > Float.epsilon)
|
|
v = 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(Vector3Float v) {
|
|
float num = v.magnitude;
|
|
Vector3Float result = zero;
|
|
if (num > Float.epsilon)
|
|
result = 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 Vector3Float operator -(Vector3Float v1) {
|
|
Vector3Float 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 Vector3Float operator -(Vector3Float v1, Vector3Float v2) {
|
|
Vector3Float 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 Vector3Float operator +(Vector3Float v1, Vector3Float v2) {
|
|
Vector3Float 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 Vector3Float Scale(Vector3Float v1, Vector3Float v2) {
|
|
return new Vector3Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical, v1.depth * v2.depth);
|
|
}
|
|
|
|
|
|
public static Vector3Float operator *(Vector3Float v1, float d) {
|
|
Vector3Float v = new(v1.horizontal * d, v1.vertical * d, v1.depth * d);
|
|
return v;
|
|
}
|
|
|
|
public static Vector3Float operator *(float d, Vector3Float v1) {
|
|
Vector3Float v = new(d * v1.horizontal, d * v1.vertical, d * v1.depth);
|
|
return v;
|
|
}
|
|
|
|
public static Vector3Float operator /(Vector3Float v1, float d) {
|
|
Vector3Float v = new(v1.horizontal / d, v1.vertical / d, v1.depth / d);
|
|
return v;
|
|
}
|
|
|
|
|
|
//public bool Equals(Vector3Float v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
|
|
|
|
public static bool operator ==(Vector3Float v1, Vector3Float v2) {
|
|
return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth);
|
|
}
|
|
|
|
public static bool operator !=(Vector3Float v1, Vector3Float v2) {
|
|
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth);
|
|
}
|
|
|
|
public override readonly bool Equals(object obj) {
|
|
if (obj is not Vector3Float v)
|
|
return false;
|
|
|
|
return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
|
|
}
|
|
|
|
public override readonly int GetHashCode() {
|
|
return HashCode.Combine(horizontal, vertical, depth);
|
|
}
|
|
|
|
/// @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(Vector3Float v1, Vector3Float 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(Vector3Float v1, Vector3Float 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 Vector3Float Cross(Vector3Float v1, Vector3Float v2) {
|
|
return new Vector3Float(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 Project the vector on another vector
|
|
/// @param v The vector to project
|
|
/// @param n The normal vecto to project on
|
|
/// @return The projected vector
|
|
public static Vector3Float Project(Vector3Float v, Vector3Float n) {
|
|
float sqrMagnitude = Dot(n, n);
|
|
if (sqrMagnitude < Float.epsilon)
|
|
return zero;
|
|
else {
|
|
float dot = Dot(v, n);
|
|
Vector3Float r = n * dot / sqrMagnitude;
|
|
return r;
|
|
}
|
|
}
|
|
|
|
/// @brief Project the vector on a plane defined by a normal orthogonal to the
|
|
/// plane.
|
|
/// @param v The vector to project
|
|
/// @param n The normal of the plane to project on
|
|
/// @return Teh projected vector
|
|
public static Vector3Float ProjectOnPlane(Vector3Float v, Vector3Float n) {
|
|
Vector3Float r = v - Project(v, n);
|
|
return r;
|
|
}
|
|
|
|
/// @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(Vector3Float v1, Vector3Float 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(Vector3Float v1, Vector3Float v2,
|
|
Vector3Float axis) {
|
|
// angle in [0,180]
|
|
AngleFloat angle = UnsignedAngle(v1, v2);
|
|
|
|
Vector3Float 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;
|
|
}
|
|
|
|
|
|
/// @brief Lerp (linear interpolation) between two vectors
|
|
/// @param v1 The starting vector
|
|
/// @param v2 The ending vector
|
|
/// @param f The interpolation distance
|
|
/// @return The lerped vector
|
|
/// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
|
|
/// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
|
|
/// between *v1* and *v2* etc.
|
|
public static Vector3Float Lerp(Vector3Float v1, Vector3Float v2, float f) {
|
|
Vector3Float v = v1 + (v2 - v1) * f;
|
|
return v;
|
|
}
|
|
|
|
}
|
|
}
|
|
//#endif |