279 lines
10 KiB
C#
279 lines
10 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
#if UNITY_5_3_OR_NEWER
|
|
using Vector3 = UnityEngine.Vector3;
|
|
#endif
|
|
|
|
namespace LinearAlgebra {
|
|
/// <summary>
|
|
/// A spherical vector
|
|
/// </summary>
|
|
/// <remark>This is a struct such that it is a value type and cannot be null
|
|
public struct Spherical {
|
|
/// <summary>
|
|
/// Create a spherical vector
|
|
/// </summary>
|
|
/// <param name="distance">The distance in meters</param>
|
|
/// <param name="direction">The direction of the vector</param>
|
|
public Spherical(float distance, Direction direction) {
|
|
if (distance > 0) {
|
|
this.distance = distance;
|
|
this.direction = direction;
|
|
}
|
|
else {
|
|
this.distance = -distance;
|
|
this.direction = -direction;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create spherical vector. All given angles are in degrees
|
|
/// </summary>
|
|
/// <param name="distance">The distance in meters</param>
|
|
/// <param name="horizontal">The horizontal angle in degrees</param>
|
|
/// <param name="vertical">The vertical angle in degrees</param>
|
|
/// <returns></returns>
|
|
public static Spherical Degrees(float distance, float horizontal, float vertical) {
|
|
Direction direction = Direction.Degrees(horizontal, vertical);
|
|
Spherical s = new(distance, direction);
|
|
return s;
|
|
}
|
|
|
|
public static Spherical Radians(float distance, float horizontal, float vertical) {
|
|
Direction direction = Direction.Radians(horizontal, vertical);
|
|
Spherical s = new(distance, direction);
|
|
return s;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The distance in meters
|
|
/// </summary>
|
|
/// @remark The distance should never be negative
|
|
public float distance;
|
|
/// <summary>
|
|
/// The direction of the vector
|
|
/// </summary>
|
|
public Direction direction;
|
|
|
|
/// <summary>
|
|
/// A spherical vector with zero degree angles and distance
|
|
/// </summary>
|
|
public readonly static Spherical zero = new(0, Direction.forward);
|
|
/// <summary>
|
|
/// A normalized forward-oriented vector
|
|
/// </summary>
|
|
public readonly static Spherical forward = new(1, Direction.forward);
|
|
|
|
#if UNITY_5_3_OR_NEWER
|
|
public static Spherical FromVector3(Vector3 v) {
|
|
float distance = v.magnitude;
|
|
Direction direction = Direction.FromVector3(v / distance);
|
|
return new Spherical(distance, direction);
|
|
}
|
|
|
|
public readonly Vector3 ToVector3() {
|
|
Vector3 v = this.direction.ToVector3();
|
|
v *= this.distance;
|
|
return v;
|
|
}
|
|
#else
|
|
public static Spherical FromVector3(Vector3Float v) {
|
|
float distance = v.magnitude;
|
|
if (distance == 0.0f)
|
|
return Spherical.zero;
|
|
else {
|
|
float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.vertical / distance)) * AngleFloat.Rad2Deg;
|
|
float horizontalAngle = (float)Math.Atan2(v.horizontal, v.depth) * AngleFloat.Rad2Deg;
|
|
return Degrees(distance, horizontalAngle, verticalAngle);
|
|
}
|
|
}
|
|
|
|
public readonly Vector3Float ToVector3() {
|
|
// float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
|
|
// float horizontalRad = this.direction.horizontal.inRadians;
|
|
// float cosVertical = (float)Math.Cos(verticalRad);
|
|
// float sinVertical = (float)Math.Sin(verticalRad);
|
|
// float cosHorizontal = (float)Math.Cos(horizontalRad);
|
|
// float sinHorizontal = (float)Math.Sin(horizontalRad);
|
|
|
|
// float x = this.distance * sinVertical * sinHorizontal;
|
|
// float y = this.distance * cosVertical;
|
|
// float z = this.distance * sinVertical * cosHorizontal;
|
|
|
|
// Vector3Float v = new(x, y, z);
|
|
Vector3Float v = this.direction.ToVector3();
|
|
v *= this.distance;
|
|
return v;
|
|
}
|
|
#endif
|
|
|
|
public override readonly string ToString() {
|
|
return $"Spherical({this.distance}, h: {this.direction.horizontal}, v: {this.direction.vertical})";
|
|
}
|
|
|
|
|
|
public readonly float magnitude => this.distance;
|
|
|
|
public Spherical normalized {
|
|
get {
|
|
Spherical r = new() {
|
|
distance = 1,
|
|
direction = this.direction
|
|
};
|
|
return r;
|
|
}
|
|
}
|
|
|
|
public static Spherical operator +(Spherical s1, Spherical s2) {
|
|
// let's do it the easy way...
|
|
// using vars to be compatible with both unity (Vector3) and native (Vector3Float)
|
|
var v1 = s1.ToVector3();
|
|
var v2 = s2.ToVector3();
|
|
var v = v1 + v2;
|
|
Spherical r = FromVector3(v);
|
|
return r;
|
|
}
|
|
|
|
public static Spherical operator *(Spherical v, float d) {
|
|
Spherical r = new(v.distance * d, v.direction);
|
|
return r;
|
|
}
|
|
|
|
public static bool operator ==(Spherical v1, Spherical v2) {
|
|
return (v1.distance == v2.distance && v1.direction == v2.direction);
|
|
}
|
|
|
|
public static bool operator !=(Spherical v1, Spherical v2) {
|
|
return (v1.distance != v2.distance || v1.direction != v2.direction);
|
|
}
|
|
|
|
public override readonly bool Equals(object o) {
|
|
if (o is Spherical s)
|
|
return this == s;
|
|
return false;
|
|
}
|
|
|
|
public override readonly int GetHashCode() {
|
|
return HashCode.Combine(this.distance, this.direction);
|
|
}
|
|
|
|
public static float Distance(Spherical v1, Spherical v2) {
|
|
// Convert degrees to radians
|
|
float thetaARadians = v1.direction.horizontal.inRadians;
|
|
float phiARadians = v1.direction.vertical.inRadians;// DegreesToRadians(phiA);
|
|
float thetaBRadians = v2.direction.horizontal.inRadians; // DegreesToRadians(thetaB);
|
|
float phiBRadians = v2.direction.vertical.inRadians; // DegreesToRadians(phiB);
|
|
|
|
// Calculate sine and cosine values
|
|
float sinPhiA = MathF.Sin(phiARadians);
|
|
float cosPhiA = MathF.Cos(phiARadians);
|
|
float sinPhiB = MathF.Sin(phiBRadians);
|
|
float cosPhiB = MathF.Cos(phiBRadians);
|
|
|
|
// Calculate the cosine of the difference in azimuthal angles
|
|
float cosThetaDifference = MathF.Cos(thetaARadians - thetaBRadians);
|
|
|
|
// Apply the spherical law of cosines
|
|
float distance = MathF.Sqrt(
|
|
v1.distance * v1.distance +
|
|
v2.distance * v2.distance -
|
|
2 * v1.distance * v2.distance * (sinPhiA * sinPhiB * cosThetaDifference + cosPhiA * cosPhiB)
|
|
);
|
|
|
|
return distance;
|
|
}
|
|
|
|
public static Spherical Average(Spherical v1, Spherical v2) {
|
|
const float EPS = 1e-6f;
|
|
|
|
// Angles in radians
|
|
float a1 = v1.direction.horizontal.inRadians;
|
|
float a2 = v2.direction.horizontal.inRadians;
|
|
float e1 = v1.direction.vertical.inRadians;
|
|
float e2 = v2.direction.vertical.inRadians;
|
|
|
|
// Fast path: exactly same direction (allowing wrap for azimuth) -> preserve exact angles
|
|
bool sameAz = MathF.Abs(MathF.IEEERemainder(a1 - a2, MathF.PI * 2f)) < EPS;
|
|
bool sameEl = MathF.Abs(e1 - e2) < EPS;
|
|
if (sameAz && sameEl) {
|
|
// Distances may differ; average distance but keep exact angles from v1
|
|
float rAvgExact = 0.5f * (v1.distance + v2.distance);
|
|
return new Spherical(rAvgExact, v1.direction);
|
|
}
|
|
|
|
// Horizontal unit-circle sum
|
|
float cx = MathF.Cos(a1) + MathF.Cos(a2);
|
|
float cy = MathF.Sin(a1) + MathF.Sin(a2);
|
|
|
|
// Vertical as z = sin(el)
|
|
float z1 = MathF.Sin(e1);
|
|
float z2 = MathF.Sin(e2);
|
|
float cz = z1 + z2;
|
|
|
|
// Magnitude of summed unit-direction vectors
|
|
float sumX = cx;
|
|
float sumY = cy;
|
|
float sumZ = cz;
|
|
float magSum = MathF.Sqrt(sumX * sumX + sumY * sumY + sumZ * sumZ);
|
|
|
|
// If the two direction unit-vectors cancel (or nearly), return zero distance.
|
|
if (magSum < EPS) {
|
|
return Spherical.Radians(0f, 0f, 0f);
|
|
}
|
|
|
|
// Normalized averaged direction components
|
|
float ux = sumX / magSum;
|
|
float uy = sumY / magSum;
|
|
float uz = sumZ / magSum;
|
|
|
|
// Compute averaged angles from normalized vector
|
|
float azAvgRad = MathF.Atan2(uy, ux);
|
|
float elAvgRad = MathF.Asin(Float.Clamp(uz, -1f, 1f));
|
|
|
|
// Average distance (arithmetic mean)
|
|
float rAvg = 0.5f * (v1.distance + v2.distance);
|
|
|
|
return Spherical.Radians(rAvg, azAvgRad, elAvgRad);
|
|
}
|
|
|
|
public static Spherical Sum(List<Spherical> vectors) {
|
|
if (vectors == null || vectors.Count == 0)
|
|
throw new ArgumentException("vectors must contain at least one element", nameof(vectors));
|
|
|
|
#if UNITY_5_3_OR_NEWER
|
|
Vector3 sum = Vector3.zero;
|
|
#else
|
|
Vector3Float sum = Vector3Float.zero;
|
|
#endif
|
|
foreach (Spherical v in vectors)
|
|
sum += v.ToVector3();
|
|
|
|
return FromVector3(sum);
|
|
}
|
|
|
|
|
|
public static Spherical Average(List<Spherical> vectors) {
|
|
if (vectors == null || vectors.Count == 0)
|
|
throw new ArgumentException("vectors must contain at least one element", nameof(vectors));
|
|
|
|
#if UNITY_5_3_OR_NEWER
|
|
Vector3 sum = Vector3.zero;
|
|
#else
|
|
Vector3Float sum = Vector3Float.zero;
|
|
#endif
|
|
int n = 0;
|
|
foreach (Spherical v in vectors) {
|
|
sum += v.ToVector3();
|
|
n++;
|
|
}
|
|
var avg = sum / n;
|
|
|
|
// if (avg.sqrMagnitude == 0f)
|
|
// return new Spherical(0f, new Direction(AngleFloat.Radians(0f), AngleFloat.Radians(0f)));
|
|
// else
|
|
return FromVector3(avg);
|
|
}
|
|
|
|
}
|
|
} |