diff --git a/src/Angle.cs b/src/Angle.cs
index 694d1b7..70f5b10 100644
--- a/src/Angle.cs
+++ b/src/Angle.cs
@@ -32,7 +32,6 @@ namespace LinearAlgebra {
}
return new AngleFloat(radians * Rad2Deg);
-
}
public static AngleFloat Revolutions(float revolutions) {
@@ -51,16 +50,14 @@ namespace LinearAlgebra {
return new AngleFloat(revolutions * 360);
}
- public float inDegrees {
- get { return this.value; }
- }
+ public readonly float inDegrees => this.value;
- public float inRadians {
- get { return this.value * Deg2Rad; }
- }
+ public readonly float inRadians => this.value * Deg2Rad;
+
+ public readonly float inRevolutions => this.value / 360.0f;
- public float inRevolutions {
- get { return this.value / 360.0f; }
+ public override string ToString() {
+ return $"{this.inDegrees} deg.";
}
public static readonly AngleFloat zero = Degrees(0);
@@ -115,6 +112,18 @@ namespace LinearAlgebra {
return a1.value != a2.value;
}
+ public override readonly bool Equals(object obj) {
+ if (obj is AngleFloat other) {
+ return this == other;
+ }
+ return false;
+ }
+
+ public override readonly int GetHashCode() {
+ return this.value.GetHashCode();
+ }
+
+
///
/// Tests if the first angle is greater than the second
///
diff --git a/src/Direction.cs b/src/Direction.cs
index ed82901..9293503 100644
--- a/src/Direction.cs
+++ b/src/Direction.cs
@@ -66,6 +66,10 @@ namespace LinearAlgebra {
return d;
}
+ public override readonly string ToString() {
+ return $"Direction(h: {this.horizontal}, v: {this.vertical})";
+ }
+
///
/// A forward direction with zero for both angles
///
@@ -98,31 +102,64 @@ namespace LinearAlgebra {
private void Normalize() {
if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) {
this.horizontal += AngleFloat.deg180;
- this.vertical = AngleFloat.deg180 - this.vertical;
+ this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees);
}
}
-#if !UNITY_5_3_OR_NEWER
+#if UNITY_5_3_OR_NEWER
///
/// Convert the direction into a carthesian vector
///
/// The carthesian vector corresponding to this direction.
- public Vector3Float ToVector3Float() {
- Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
- Vector3Float v = q * Vector3Float.forward;
- return v;
+ public readonly UnityEngine.Vector3 ToVector3() {
+ // Convert degrees to radians
+ float radH = this.horizontal.inRadians;
+ float radV = this.vertical.inRadians;
+
+ // Calculate Vector
+ float cosV = MathF.Cos(radV);
+ float x = cosV * MathF.Cos(radH);
+ float y = MathF.Sin(radV);
+ float z = cosV * MathF.Sin(radH);
+
+ return new UnityEngine.Vector3(x, y, z);
}
-#else
+
+ ///
+ /// Convert a carthesian vector into a direction
+ ///
+ /// The carthesian vector
+ /// The direction
+ /// Information about the length of the carthesian vector is not
+ /// included in this transformation
+ public static Direction FromVector3(UnityEngine.Vector3 v) {
+ AngleFloat horizontal = AngleFloat.Atan2(v.x, v.z);
+ AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.y);
+ Direction d = new(horizontal, vertical);
+ return d;
+ }
+#else
///
/// Convert the direction into a carthesian vector
///
/// The carthesian vector corresponding to this direction.
- public UnityEngine.Vector3 ToVector3() {
- UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
- UnityEngine.Vector3 v = q * UnityEngine.Vector3.forward;
- return v;
+ public readonly Vector3Float ToVector3() {
+ // Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0);
+ // Vector3Float v = q * Vector3Float.forward;
+ // return v;
+
+ float radH = this.horizontal.inRadians;
+ float radV = this.vertical.inRadians;
+
+ float cosV = MathF.Cos(radV);
+ float sinV = MathF.Sin(radV);
+
+ float horizontal = cosV * MathF.Sin(radH);
+ float vertical = sinV;
+ float depth = cosV * MathF.Cos(radH);
+
+ return new Vector3Float(horizontal, vertical, depth);
}
-#endif
///
/// Convert a carthesian vector into a direction
@@ -137,6 +174,8 @@ namespace LinearAlgebra {
Direction d = new(horizontal, vertical);
return d;
}
+#endif
+
///
/// Tests the equality of two directions
@@ -176,6 +215,26 @@ namespace LinearAlgebra {
return HashCode.Combine(horizontal, vertical);
}
+ public static AngleFloat UnsignedAngle(Direction d1, Direction d2) {
+ // Convert angles from degrees to radians
+ float horizontal1Rad = d1.horizontal.inRadians;
+ float vertical1Rad = d1.vertical.inRadians;
+
+ float horizontal2Rad = d2.horizontal.inRadians;
+ float vertical2Rad = d2.vertical.inRadians;
+
+ // Calculate the cosine of the angle using the spherical law of cosines
+ float cosTheta = MathF.Sin(vertical1Rad) * MathF.Sin(vertical2Rad) +
+ MathF.Cos(vertical1Rad) * MathF.Cos(vertical2Rad) *
+ MathF.Cos(horizontal1Rad - horizontal2Rad);
+
+ // Clip cosTheta to the valid range for acos
+ cosTheta = Float.Clamp(cosTheta, -1.0f, 1.0f);
+
+ // Calculate the angle
+ AngleFloat angle = AngleFloat.Acos(cosTheta);
+ return angle;
+ }
}
}
\ No newline at end of file
diff --git a/src/Quaternion.cs b/src/Quaternion.cs
index 670c0a4..7936843 100644
--- a/src/Quaternion.cs
+++ b/src/Quaternion.cs
@@ -180,11 +180,7 @@ namespace LinearAlgebra {
/// The resulting quaternion
/// Rotation are appied in the order Z, X, Y.
public static Quaternion Euler(Vector3Float angles) {
- // return Quaternion.FromEulerRad(angles * AngleFloat.Deg2Rad);
- // }
- // private static Quaternion FromEulerRad(Vector3Float euler) {
Vector3Float euler = angles * AngleFloat.Deg2Rad;
- // euler.x = pitch, euler.y = yaw, euler.z = roll (radians)
float cx = MathF.Cos(euler.horizontal * 0.5f);
float sx = MathF.Sin(euler.horizontal * 0.5f);
float cy = MathF.Cos(euler.vertical * 0.5f);
@@ -199,28 +195,6 @@ namespace LinearAlgebra {
q.y = sy * cx * cz - cy * sx * sz;
q.z = cy * cx * sz - sy * sx * cz;
return q;
- // float yaw = euler.horizontal;
- // float pitch = euler.vertical;
- // float roll = euler.depth;
- // float rollOver2 = roll * 0.5f;
- // float sinRollOver2 = MathF.Sin(rollOver2);
- // float cosRollOver2 = MathF.Cos(rollOver2);
- // float pitchOver2 = pitch * 0.5f;
- // float sinPitchOver2 = MathF.Sin(pitchOver2);
- // float cosPitchOver2 = MathF.Cos(pitchOver2);
- // float yawOver2 = yaw * 0.5f;
- // float sinYawOver2 = MathF.Sin(yawOver2);
- // float cosYawOver2 = MathF.Cos(yawOver2);
- // Quaternion result;
- // result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
- // sinYawOver2 * sinPitchOver2 * sinRollOver2;
- // result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
- // cosYawOver2 * sinPitchOver2 * sinRollOver2;
- // result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
- // sinYawOver2 * cosPitchOver2 * sinRollOver2;
- // result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
- // sinYawOver2 * sinPitchOver2 * cosRollOver2;
- // return result;
}
///
diff --git a/src/Spherical.cs b/src/Spherical.cs
index de06d24..3a76085 100644
--- a/src/Spherical.cs
+++ b/src/Spherical.cs
@@ -1,23 +1,22 @@
using System;
+using System.Collections.Generic;
+
#if UNITY_5_3_OR_NEWER
using Vector3 = UnityEngine.Vector3;
#endif
-namespace LinearAlgebra
-{
+namespace LinearAlgebra {
///
/// A spherical vector
///
/// This is a struct such that it is a value type and cannot be null
- public struct Spherical
- {
+ public struct Spherical {
///
/// Create a spherical vector
///
/// The distance in meters
/// The direction of the vector
- public Spherical(float distance, Direction direction)
- {
+ public Spherical(float distance, Direction direction) {
this.distance = distance;
this.direction = direction;
}
@@ -29,15 +28,13 @@ namespace LinearAlgebra
/// The horizontal angle in degrees
/// The vertical angle in degrees
///
- public static Spherical Degrees(float distance, float horizontal, float vertical)
- {
+ 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)
- {
+ public static Spherical Radians(float distance, float horizontal, float vertical) {
Direction direction = Direction.Radians(horizontal, vertical);
Spherical s = new(distance, direction);
return s;
@@ -62,79 +59,19 @@ namespace LinearAlgebra
///
public readonly static Spherical forward = new(1, Direction.forward);
-
- // public static Spherical FromVector3Float(Vector3Float v) {
- // float distance = v.magnitude;
- // if (distance == 0.0f)
- // return Spherical.zero;
- // else {
- // float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
- // float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
- // return Spherical.Degrees(distance, horizontalAngle, verticalAngle);
- // }
- // }
-
- public static Spherical FromVector3Float(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 Vector3Float ToVector3Float() {
- // float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
- // float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
- // 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);
- // return v;
- // }
-
- public Vector3Float ToVector3Float()
- {
- 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);
- return v;
- }
#if UNITY_5_3_OR_NEWER
- public static Spherical FromVector3(Vector3 v)
- {
+ public static Spherical FromVector3(Vector3 v) {
float distance = v.magnitude;
if (distance == 0.0f)
return Spherical.zero;
- else
- {
+ else {
float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * AngleFloat.Rad2Deg;
float horizontalAngle = (float)Math.Atan2(v.x, v.z) * AngleFloat.Rad2Deg;
return Degrees(distance, horizontalAngle, verticalAngle);
}
}
- public Vector3 ToVector3()
- {
+ public readonly Vector3 ToVector3() {
float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
float horizontalRad = this.direction.horizontal.inRadians;
float cosVertical = (float)Math.Cos(verticalRad);
@@ -149,21 +86,40 @@ namespace LinearAlgebra
Vector3 v = new(x, y, z);
return v;
}
-#endif
-
- public float magnitude
- {
- get
- {
- return this.distance;
+#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 Spherical normalized
- {
- get
- {
- Spherical r = new()
- {
+
+ 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);
+ return v;
+ }
+#endif
+
+ public readonly float magnitude => this.distance;
+
+ public Spherical normalized {
+ get {
+ Spherical r = new() {
distance = 1,
direction = this.direction
};
@@ -171,15 +127,251 @@ namespace LinearAlgebra
}
}
- public static Spherical operator +(Spherical s1, Spherical s2)
- {
+ public static Spherical operator +(Spherical s1, Spherical s2) {
// let's do it the easy way...
- Vector3Float v1 = s1.ToVector3Float();
- Vector3Float v2 = s2.ToVector3Float();
- Vector3Float v = v1 + v2;
- Spherical r = FromVector3Float(v);
+ // 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 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 Average(List vectors) {
+ // float sumSinPhiCosTheta = 0.0f;
+ // float sumSinPhiSinTheta = 0.0f;
+ // float sumCosPhi = 0.0f;
+
+ // int n = vectors.Count;
+
+ // // Step 1: Accumulate sine and cosine components
+ // foreach(Spherical v in vectors) {
+ // float sinHorizontal = AngleFloat.Sin(v.direction.horizontal);
+ // sumSinPhiCosTheta += v.distance * sinHorizontal * AngleFloat.Cos(v.direction.vertical);
+ // sumSinPhiSinTheta += v.distance * sinHorizontal * AngleFloat.Sin(v.direction.vertical);
+ // sumCosPhi += v.distance * AngleFloat.Cos(v.direction.horizontal);
+ // }
+
+ // // Step 2: Calculate average components
+ // float avgSinPhiCosTheta = sumSinPhiCosTheta / n;
+ // float avgSinPhiSinTheta = sumSinPhiSinTheta / n;
+ // float avgCosPhi = sumCosPhi / n;
+
+ // // Step 3: Calculate the magnitude of the average vector
+ // float rAvg = MathF.Sqrt(avgSinPhiCosTheta * avgSinPhiCosTheta +
+ // avgSinPhiSinTheta * avgSinPhiSinTheta +
+ // avgCosPhi * avgCosPhi);
+
+ // // Step 4: Calculate average angles
+ // AngleFloat horizontalAvg = AngleFloat.Acos(avgCosPhi / rAvg); // Handle rAvg != 0 case
+ // AngleFloat verticalAvg = AngleFloat.Atan2(avgSinPhiSinTheta, avgSinPhiCosTheta);
+
+ // return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg));
+
+ if (vectors == null || vectors.Count == 0)
+ throw new ArgumentException("vectors must contain at least one element", nameof(vectors));
+
+ float sumX = 0f, sumY = 0f, sumZ = 0f;
+ int n = vectors.Count;
+
+ foreach (var v in vectors) {
+ // AngleFloat -> radians; assume AngleFloat provides Radians property
+ float theta = v.direction.horizontal.inRadians; // azimuth
+ float phi = v.direction.vertical.inRadians; // elevation
+
+ float cosPhi = MathF.Cos(phi);
+ float sinPhi = MathF.Sin(phi);
+ float cosTheta = MathF.Cos(theta);
+ float sinTheta = MathF.Sin(theta);
+
+ float x = v.distance * cosPhi * cosTheta;
+ float y = v.distance * cosPhi * sinTheta;
+ float z = v.distance * sinPhi;
+
+ sumX += x;
+ sumY += y;
+ sumZ += z;
+ }
+
+ float avgX = sumX / n;
+ float avgY = sumY / n;
+ float avgZ = sumZ / n;
+
+ float rAvg = MathF.Sqrt(avgX * avgX + avgY * avgY + avgZ * avgZ);
+
+ if (rAvg == 0f) {
+ return new Spherical(0f, new Direction(AngleFloat.Radians(0f), AngleFloat.Radians(0f)));
+ }
+
+ // elevation = asin(z / r)
+ AngleFloat verticalAvg = AngleFloat.Asin(avgZ / rAvg); // -90..90
+ // azimuth = atan2(y, x) -> -pi..pi
+ AngleFloat horizontalAvg = AngleFloat.Atan2(avgY, avgX); // -180..180
+
+ return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg));
+ }
+
+
+/*
+ public static Spherical Average(IEnumerable vectors) {
+ const float EPS = 1e-6f;
+ if (vectors == null) throw new ArgumentNullException(nameof(vectors));
+
+ float sumRx = 0f, sumRy = 0f, sumRz = 0f;
+ float sumDistances = 0f;
+ int count = 0;
+
+ bool firstSet = false;
+ float firstAz = 0f, firstEl = 0f;
+ bool allSameDirection = true;
+
+ foreach (var v in vectors) {
+ float az = v.direction.horizontal.inRadians; // horizontal (azimuth)
+ float el = v.direction.vertical.inRadians; // vertical (elevation)
+
+ if (!firstSet) {
+ firstSet = true;
+ firstAz = az;
+ firstEl = el;
+ }
+ else {
+ if (MathF.Abs(MathF.IEEERemainder(az - firstAz, MathF.PI * 2f)) >= EPS ||
+ MathF.Abs(el - firstEl) >= EPS) {
+ allSameDirection = false;
+ }
+ }
+
+ float cosEl = MathF.Cos(el);
+ float ux = cosEl * MathF.Cos(az); // x
+ float uy = cosEl * MathF.Sin(az); // y
+ float uz = MathF.Sin(el); // z
+
+ sumRx += v.distance * ux;
+ sumRy += v.distance * uy;
+ sumRz += v.distance * uz;
+
+ sumDistances += v.distance;
+ count++;
+ }
+
+ if (count == 0) throw new ArgumentException("Sequence contains no elements", nameof(vectors));
+
+ // All directions equal -> preserve exact angles, average distance
+ if (allSameDirection) {
+ float rAvg = sumDistances / count;
+ return new Spherical(rAvg, Direction.Radians(firstAz, firstEl));
+ }
+
+ // Total vector sum V
+ float Vx = sumRx;
+ float Vy = sumRy;
+ float Vz = sumRz;
+ float Vmag = MathF.Sqrt(Vx * Vx + Vy * Vy + Vz * Vz);
+
+ if (Vmag < EPS) {
+ // Directions cancel out -> zero distance, angles arbitrary
+ return Spherical.Radians(0f, 0f, 0f);
+ }
+
+ float azAvg = MathF.Atan2(Vy, Vx);
+ float elAvg = MathF.Asin(Float.Clamp(Vz / Vmag, -1f, 1f));
+ float rAvgFinal = Vmag / count;
+
+ return Spherical.Radians(rAvgFinal, azAvg, elAvg);
+ }
+*/
+
}
}
\ No newline at end of file
diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs
index 4437c4f..df6e048 100644
--- a/src/SwingTwist.cs
+++ b/src/SwingTwist.cs
@@ -1,6 +1,6 @@
-#if !UNITY_5_3_OR_NEWER
-using UnityEngine;
-#endif
+// #if !UNITY_5_3_OR_NEWER
+// using UnityEngine;
+// #endif
namespace LinearAlgebra {
@@ -46,7 +46,45 @@ namespace LinearAlgebra {
return s;
}
-#if !UNITY_5_3_OR_NEWER
+#if UNITY_5_3_OR_NEWER
+ ///
+ /// A zero angle rotation
+ ///
+ public static readonly SwingTwist zero = Degrees(0, 0, 0);
+
+ public Spherical ToAngleAxis() {
+ UnityEngine.Quaternion q = this.ToQuaternion();
+ q.ToAngleAxis(out float angle, out UnityEngine.Vector3 axis);
+ Direction direction = Direction.FromVector3(axis);
+
+ Spherical r = new(angle, direction);
+ return r;
+ }
+
+ public static SwingTwist FromAngleAxis(Spherical r) {
+ UnityEngine.Vector3 vectorAxis = r.direction.ToVector3();
+ UnityEngine.Quaternion q = UnityEngine.Quaternion.AngleAxis(r.distance, vectorAxis);
+ return FromQuaternion(q);
+ }
+
+ ///
+ /// Convert a quaternion in a swing/twist rotation
+ ///
+ /// The quaternion to convert
+ /// The swing/twist rotation
+ public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) {
+ UnityEngine.Vector3 angles = q.eulerAngles;
+ SwingTwist r = Degrees(angles.y, -angles.x, -angles.z);
+ return r;
+ }
+
+ public UnityEngine.Quaternion ToQuaternion() {
+ UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees,
+ this.swing.horizontal.inDegrees,
+ this.twist.inDegrees);
+ return q;
+ }
+#else
///
/// A zero angle rotation
///
@@ -92,25 +130,7 @@ namespace LinearAlgebra {
return r;
}
#endif
-#if UNITY_5_3_OR_NEWER
- ///
- /// Convert a quaternion in a swing/twist rotation
- ///
- /// The quaternion to convert
- /// The swing/twist rotation
- public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) {
- UnityEngine.Vector3 angles = q.eulerAngles;
- SwingTwist r = Degrees(angles.y, -angles.x, -angles.z);
- return r;
- }
-
- public UnityEngine.Quaternion ToUnityQuaternion() {
- UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees,
- this.swing.horizontal.inDegrees,
- this.twist.inDegrees);
- return q;
- }
-#endif
+
}
}
\ No newline at end of file
diff --git a/src/Vector2Float.cs b/src/Vector2Float.cs
index e0418a8..ac1867c 100644
--- a/src/Vector2Float.cs
+++ b/src/Vector2Float.cs
@@ -324,27 +324,13 @@ namespace LinearAlgebra {
public static Vector2Float Scale(Vector2Float v1, Vector2Float v2) {
return new Vector2Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical);
}
-
- /*
+
///
/// Tests if the vector has equal values as the given vector
///
/// The vector to compare to
/// true if the vector values are equal
- public bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical;
-
- ///
- /// Tests if the vector is equal to the given object
- ///
- /// The object to compare to
- /// false when the object is not a Vector2 or does not have equal values
- public override bool Equals(object obj) {
- if (!(obj is Vector2Float v))
- return false;
-
- return (horizontal == v.horizontal && vertical == v.vertical);
- }
- */
+ //public readonly bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical;
///
/// Tests if the two vectors have equal values
@@ -372,15 +358,25 @@ namespace LinearAlgebra {
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
}
- /*
+ ///
+ /// Tests if the vector is equal to the given object
+ ///
+ /// The object to compare to
+ /// false when the object is not a Vector2 or does not have equal values
+ public override readonly bool Equals(object obj) {
+ if (obj is not Vector2Float v)
+ return false;
+
+ return (horizontal == v.horizontal && vertical == v.vertical);
+ }
+
///
/// Get an hash code for the vector
///
/// The hash code
- public override int GetHashCode() {
- return (horizontal, vertical).GetHashCode();
+ public override readonly int GetHashCode() {
+ return HashCode.Combine(horizontal, vertical);
}
- */
///
/// Get the distance between two vectors
diff --git a/src/Vector2Int.cs b/src/Vector2Int.cs
index 0eca7dc..ed68e8b 100644
--- a/src/Vector2Int.cs
+++ b/src/Vector2Int.cs
@@ -60,17 +60,6 @@ namespace LinearAlgebra {
/// true if the vector values are equal
public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical;
- ///
- /// Tests if the vector is equal to the given object
- ///
- /// The object to compare to
- /// false when the object is not a Vector2 or does not have equal values
- public override readonly bool Equals(object obj) {
- if (obj is not Vector2Int v)
- return false;
-
- return (this.horizontal == v.horizontal && this.vertical == v.vertical);
- }
*/
///
@@ -98,6 +87,26 @@ namespace LinearAlgebra {
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
}
+ ///
+ /// Tests if the vector is equal to the given object
+ ///
+ /// The object to compare to
+ /// false when the object is not a Vector2 or does not have equal values
+ public override readonly bool Equals(object obj) {
+ if (obj is not Vector2Int v)
+ return false;
+
+ return (this.horizontal == v.horizontal && this.vertical == v.vertical);
+ }
+
+ ///
+ /// Get an hash code for the vector
+ ///
+ /// The hash code
+ public override readonly int GetHashCode() {
+ return HashCode.Combine(horizontal, vertical);
+ }
+
public readonly float sqrMagnitude => this.horizontal * this.horizontal + this.vertical * this.vertical;
public static float SqrMagnitudeOf(Vector2Int v) {
diff --git a/src/Vector3Float.cs b/src/Vector3Float.cs
index bff0936..d8208d3 100644
--- a/src/Vector3Float.cs
+++ b/src/Vector3Float.cs
@@ -250,30 +250,22 @@ namespace LinearAlgebra {
public static Vector3Float operator *(Vector3Float v1, float d) {
- Vector3Float v = new Vector3Float(v1.horizontal * d, v1.vertical * d, v1.depth * 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 Vector3Float(d * v1.horizontal, d * v1.vertical, d * v1.depth);
+ 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 Vector3Float(v1.horizontal / d, v1.vertical / d, v1.depth / 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 override bool Equals(object obj) {
- if (!(obj is Vector3Float v))
- return false;
-
- return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth);
- }
- */
+
+ //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);
@@ -283,9 +275,16 @@ namespace LinearAlgebra {
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth);
}
- // public override int GetHashCode() {
- // return (horizontal, vertical, depth).GetHashCode();
- // }
+ 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
diff --git a/test/AngleTest.cs b/test/AngleTest.cs
index 787130d..8362d82 100644
--- a/test/AngleTest.cs
+++ b/test/AngleTest.cs
@@ -24,6 +24,10 @@ namespace LinearAlgebra.Test {
a = AngleFloat.Degrees(angle);
Assert.AreEqual(-90, a.inDegrees);
+ angle = -270.0f;
+ a = AngleFloat.Degrees(angle);
+ Assert.AreEqual(90, a.inDegrees);
+
// Radians
angle = 0.0f;
a = AngleFloat.Radians(angle);
diff --git a/test/DirectionTest.cs b/test/DirectionTest.cs
index 146c9df..8fe3b93 100644
--- a/test/DirectionTest.cs
+++ b/test/DirectionTest.cs
@@ -33,6 +33,13 @@ namespace LinearAlgebra.Test {
Assert.AreEqual(30, d.vertical.inDegrees, 0.0001f);
}
+ [Test]
+ public void DegreesNormalize1() {
+ Direction d = Direction.Degrees(112, 91);
+ Assert.AreEqual(-68, d.horizontal.inDegrees, 0.0001f);
+ Assert.AreEqual(89, d.vertical.inDegrees, 0.0001f);
+ }
+
[Test]
public void RadiansEquivalentToDegreesConversion() {
Direction d1 = Direction.Radians((float)Math.PI / 3, (float)Math.PI / 4);
@@ -68,6 +75,15 @@ namespace LinearAlgebra.Test {
Assert.AreEqual(0, v.depth, 0.0001f);
}
+ [Test]
+ public void ToVector3Left() {
+ Direction d = Direction.left;
+ Vector3Float v = d.ToVector3();
+ Assert.AreEqual(-1, v.horizontal, 0.0001f);
+ Assert.AreEqual(0, v.vertical, 0.0001f);
+ Assert.AreEqual(0, v.depth, 0.0001f);
+ }
+
[Test]
public void FromVector3Forward() {
Vector3Float v = new(0, 0, 1);
@@ -85,6 +101,15 @@ namespace LinearAlgebra.Test {
Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f);
}
+ [Test]
+ public void ToVector3AndBack2() {
+ Direction d1 = Direction.Degrees(135, 85);
+ Vector3Float v = d1.ToVector3();
+ Direction d2 = Direction.FromVector3(v);
+ Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f);
+ Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f);
+ }
+
[Test]
public void Compare() {
Direction d1 = Direction.Degrees(45, 135);
diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs
index 125cdb1..d7553dd 100644
--- a/test/SphericalTest.cs
+++ b/test/SphericalTest.cs
@@ -1,5 +1,6 @@
#if !UNITY_5_6_OR_NEWER
using System;
+using System.Collections.Generic;
using NUnit.Framework;
namespace LinearAlgebra.Test {
@@ -48,6 +49,191 @@ namespace LinearAlgebra.Test {
Assert.AreEqual(45.0f, r.direction.horizontal.inDegrees, "Addition(1 0 90)");
Assert.AreEqual(45.0f, r.direction.vertical.inDegrees, 1.0E-05F, "Addition(1 0 90)");
}
+
+ [Test]
+ public void Average2_IdenticalVectors() {
+ Direction dir = Direction.Radians(MathF.PI / 4f, MathF.PI / 6f);
+ Spherical v = new(2.5f, dir);
+
+ Spherical avg = Spherical.Average(v, v);
+
+ Assert.AreEqual(2.5f, avg.distance, 1e-5f);
+ Assert.AreEqual(dir.horizontal, avg.direction.horizontal);
+ Assert.AreEqual(dir.vertical, avg.direction.vertical);
+ }
+
+ [Test]
+ public void Average2_OppositeUnitVectors() {
+ // Two opposite vectors: same distance, horizontal opposite (pi apart), same vertical
+ Spherical v1 = Spherical.Radians(1f, 0f, 0f);
+ Spherical v2 = Spherical.Radians(1f, MathF.PI, 0f);
+ Spherical avg = Spherical.Average(v1, v2);
+
+ Assert.AreEqual(0f, avg.distance, 1e-4f);
+ // When distance is zero, angles may be undefined; allow any angle but ensure near-zero magnitude
+ }
+
+ [Test]
+ public void Average2_WeightedByDistance() {
+ // Two vectors same direction but different distances -> weighted average distance
+ Direction dir = Direction.Radians(MathF.PI / 3f, MathF.PI / 4f);
+ Spherical a = new(1f, dir);
+ Spherical b = new(3f, dir);
+ Spherical avg = Spherical.Average(a, b);
+
+ // average distance should be (1+3)/2 = 2
+ Assert.AreEqual(2f, avg.distance, 1e-5f);
+ Assert.AreEqual(dir.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f);
+ Assert.AreEqual(dir.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f);
+ }
+
+ [Test]
+ public void Average2_OppositeButNotExact_NotZero() {
+ // Nearly opposite but not exact; expect a valid averaged direction and averaged distance
+ Direction d1 = Direction.Radians(0f, 0f);
+ Direction d2 = Direction.Radians(MathF.PI - 1e-3f, 0.0f); // slight offset
+ Spherical v1 = new(2.0f, d1);
+ Spherical v2 = new(4.0f, d2);
+
+ Spherical avg = Spherical.Average(v1, v2);
+
+ // Distance is arithmetic mean
+ Assert.AreEqual(3.0f, avg.distance, 1e-5f);
+
+ // Averaged azimuth should be near +pi/2 or -pi/2? we can check it's not NaN and unit-vector properties hold
+ float ux = MathF.Cos(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians);
+ float uy = MathF.Sin(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians);
+ float uz = MathF.Sin(avg.direction.vertical.inRadians);
+ float mag = MathF.Sqrt(ux * ux + uy * uy + uz * uz);
+ Assert.IsTrue(mag > 0.999f && mag < 1.001f);
+
+ }
+
+ [Test]
+ public void Average2_BasicAverageDirectionAndDistance() {
+ // Two different directions not cancelling: expect vector-average result
+ Direction d1 = Direction.Radians(MathF.PI / 6f, MathF.PI / 12f); // 30°, 15°
+ Direction d2 = Direction.Radians(MathF.PI / 3f, MathF.PI / 18f); // 60°, 10°
+ Spherical v1 = new(2.0f, d1);
+ Spherical v2 = new(4.0f, d2);
+
+ Spherical avg = Spherical.Average(v1, v2);
+
+ // Distance is arithmetic mean
+ Assert.AreEqual(3.0f, avg.distance, 1e-5f);
+
+ // Check averaged unit-vector equals normalized sum of unit vectors computed here
+ float a1 = d1.horizontal.inRadians;
+ float a2 = d2.horizontal.inRadians;
+ float e1 = d1.vertical.inRadians;
+ float e2 = d2.vertical.inRadians;
+
+ float cx = MathF.Cos(a1) + MathF.Cos(a2);
+ float cy = MathF.Sin(a1) + MathF.Sin(a2);
+ float z1 = MathF.Sin(e1);
+ float z2 = MathF.Sin(e2);
+ float cz = z1 + z2;
+ float mag = MathF.Sqrt(cx * cx + cy * cy + cz * cz);
+ Assert.IsTrue(mag > 1e-6f);
+
+ float ux = cx / mag;
+ float uy = cy / mag;
+ float uz = cz / mag;
+
+ // Reconstruct direction from avg result
+ float uxAvg = MathF.Cos(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians);
+ float uyAvg = MathF.Sin(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians);
+ float uzAvg = MathF.Sin(avg.direction.vertical.inRadians);
+
+ Assert.AreEqual(ux, uxAvg, 1e-4f);
+ Assert.AreEqual(uy, uyAvg, 1e-4f);
+ Assert.AreEqual(uz, uzAvg, 1e-4f);
+ }
+
+ [Test]
+ public void Average_IdenticalVectors() {
+ var dir = Direction.Radians(MathF.PI / 4f, MathF.PI / 6f);
+ var v = new Spherical(2.5f, dir);
+ var list = new List { v, v, v };
+
+ var avg = Spherical.Average(list);
+
+ Assert.AreEqual(2.5f, avg.distance, 1e-5f);
+ Assert.AreEqual(dir.horizontal, avg.direction.horizontal);
+ Assert.AreEqual(dir.vertical, avg.direction.vertical);
+ }
+
+ [Test]
+ public void Average_SingleElement() {
+ Spherical s = Spherical.Radians(1.234f, 0.3f, -0.7f);
+ Spherical avg = Spherical.Average([s]);
+
+ Assert.AreEqual(s.distance, avg.distance, 1e-5f);
+ Assert.AreEqual(s.direction.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f);
+ Assert.AreEqual(s.direction.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f);
+ }
+
+ [Test]
+ public void Average_OppositeUnitVectors() {
+ // Two opposite vectors: same distance, horizontal opposite (pi apart), same vertical
+ Spherical v1 = Spherical.Radians(1f, 0f, 0f);
+ Spherical v2 = Spherical.Radians(1f, MathF.PI, 0f);
+ Spherical avg = Spherical.Average([v1, v2]);
+
+ Assert.AreEqual(0f, avg.distance, 1e-4f);
+ // When distance is zero, angles may be undefined; allow any angle but ensure near-zero magnitude
+ }
+
+ [Test]
+ public void Average_WeightedByDistance() {
+ // Two vectors same direction but different distances -> weighted average distance
+ Direction dir = Direction.Radians(MathF.PI / 3f, MathF.PI / 4f);
+ Spherical a = new(1f, dir);
+ Spherical b = new(3f, dir);
+ Spherical avg = Spherical.Average([a, b]);
+
+ // average distance should be (1+3)/2 = 2
+ Assert.AreEqual(2f, avg.distance, 1e-5f);
+ Assert.AreEqual(dir.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f);
+ Assert.AreEqual(dir.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f);
+ }
+
+ [Test]
+ public void Average_AxisSymmetricAroundVertical() {
+ // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same elevation (vertical) angle phi
+ float phi = MathF.PI / 6f; // elevation from horizontal plane
+ var dirs = new List {
+ new(1f, Direction.Radians(0f, phi)),
+ new(1f, Direction.Radians(MathF.PI/2, phi)),
+ new(1f, Direction.Radians(MathF.PI, phi)),
+ new(1f, Direction.Radians(3*MathF.PI/2, phi))
+ };
+
+ Spherical avg = Spherical.Average(dirs);
+
+ // rAvg should equal r * sin(elevation) = sin(phi)
+ Assert.AreEqual(MathF.Sin(phi), avg.distance, 1e-4f);
+ // vertical angle undefined when horizontal xy components cancel; allow any angle but ensure r matches
+ }
+
+ [Test]
+ public void Average_AxisSymmetricAroundVertical2() {
+ // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same polar angle from vertical (alpha)
+ float alpha = MathF.PI / 6f; // polar angle from vertical
+ float elevation = MathF.PI / 2f - alpha; // convert polar-from-vertical to elevation
+ var dirs = new List {
+ new(1f, Direction.Radians(0f, elevation)),
+ new(1f, Direction.Radians(MathF.PI/2, elevation)),
+ new(1f, Direction.Radians(MathF.PI, elevation)),
+ new(1f, Direction.Radians(3*MathF.PI/2, elevation))
+ };
+
+ Spherical avg = Spherical.Average(dirs);
+
+ // rAvg should equal r * sin(elevation) which equals cos(alpha)
+ Assert.AreEqual(MathF.Cos(alpha), avg.distance, 1e-4f);
+ }
+
}
}
#endif
\ No newline at end of file