NanoBrain/NanoBrain-unitypackage
NanoBrain/NanoBrain-unitypackage
2
0
Fork 0
Code Issues Pull Requests Actions Packages Releases Activity

Squashed 'Assets/NanoBrain/LinearAlgebra/' changes from 15c08f2..672f8bf

Browse Source 
672f8bf Spherical Average
a278b7d Fix/Improve ToVector3
09d34d1 Prepare for spherical average
b19e504 (A little) Performance improvements
2b0433f Fix normalizing direction
3e115cc Fix Direction.ToVector3
0eeedd2 Vector3 conversion fixes
3024562 Fix Unity warnings
aa23d57 Fix roaming boid
cdfe039 Improve Unity compatibility

git-subtree-dir: Assets/NanoBrain/LinearAlgebra
git-subtree-split: 672f8bfca1b1e0bc312df41142fa3c4447ce6dba
This commit is contained in:
Pascal Serrarens 2026-01-07 11:33:48 +01:00
parent 220e1e4ead
commit 841d923fed
11 changed files with 682 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/Angle.cs
View File

@ -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();
}
/// <summary>
/// Tests if the first angle is greater than the second
/// </summary>

83
src/Direction.cs
View File

@ -66,6 +66,10 @@ namespace LinearAlgebra {
return d;
}
public override readonly string ToString() {
return $"Direction(h: {this.horizontal}, v: {this.vertical})";
}
/// <summary>
/// A forward direction with zero for both angles
/// </summary>
@ -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
/// <summary>
/// Convert the direction into a carthesian vector
/// </summary>
/// <returns>The carthesian vector corresponding to this direction.</returns>
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
/// <summary>
/// Convert a carthesian vector into a direction
/// </summary>
/// <param name="v">The carthesian vector</param>
/// <returns>The direction</returns>
/// <remarks>Information about the length of the carthesian vector is not
/// included in this transformation</remarks>
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
/// <summary>
/// Convert the direction into a carthesian vector
/// </summary>
/// <returns>The carthesian vector corresponding to this direction.</returns>
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
/// <summary>
/// Convert a carthesian vector into a direction
@ -137,6 +174,8 @@ namespace LinearAlgebra {
Direction d = new(horizontal, vertical);
return d;
}
#endif
/// <summary>
/// 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;
}
}
}

26
src/Quaternion.cs
View File

@ -180,11 +180,7 @@ namespace LinearAlgebra {
/// <returns>The resulting quaternion</returns>
/// 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;
}
/// <summary>

376
src/Spherical.cs
View File

@ -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 {
/// <summary>
/// A spherical vector
/// </summary>
/// <remark>This is a struct such that it is a value type and cannot be null
public struct Spherical
{
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)
{
public Spherical(float distance, Direction direction) {
this.distance = distance;
this.direction = direction;
}
@ -29,15 +28,13 @@ namespace LinearAlgebra
/// <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)
{
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
/// </summary>
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<Spherical> 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<Spherical> 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);
}
*/
}
}

66
src/SwingTwist.cs
View File

@ -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
/// <summary>
/// A zero angle rotation
/// </summary>
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);
}
/// <summary>
/// Convert a quaternion in a swing/twist rotation
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>The swing/twist rotation</returns>
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
/// <summary>
/// A zero angle rotation
/// </summary>
@ -92,25 +130,7 @@ namespace LinearAlgebra {
return r;
}
#endif
#if UNITY_5_3_OR_NEWER
/// <summary>
/// Convert a quaternion in a swing/twist rotation
/// </summary>
/// <param name="q">The quaternion to convert</param>
/// <returns>The swing/twist rotation</returns>
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
}
}

36
src/Vector2Float.cs
View File

@ -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);
}
/*
/// <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(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical;
/// <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 Vector2Float v))
return false;
return (horizontal == v.horizontal && vertical == v.vertical);
}
*/
//public readonly bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical;
/// <summary>
/// Tests if the two vectors have equal values
@ -372,15 +358,25 @@ namespace LinearAlgebra {
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
}
/*
/// <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 readonly bool Equals(object obj) {
if (obj is not Vector2Float v)
return false;
return (horizontal == v.horizontal && vertical == v.vertical);
}
/// <summary>
/// Get an hash code for the vector
/// </summary>
/// <returns>The hash code</returns>
public override int GetHashCode() {
return (horizontal, vertical).GetHashCode();
public override readonly int GetHashCode() {
return HashCode.Combine(horizontal, vertical);
}
*/
/// <summary>
/// Get the distance between two vectors

31
src/Vector2Int.cs
View File

@ -60,17 +60,6 @@ namespace LinearAlgebra {
/// <returns><em>true</em> if the vector values are equal</returns>
public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical;
/// <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 readonly bool Equals(object obj) {
if (obj is not Vector2Int v)
return false;
return (this.horizontal == v.horizontal && this.vertical == v.vertical);
}
*/
/// <summary>
@ -98,6 +87,26 @@ namespace LinearAlgebra {
return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical);
}
/// <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 readonly bool Equals(object obj) {
if (obj is not Vector2Int v)
return false;
return (this.horizontal == v.horizontal && this.vertical == v.vertical);
}
/// <summary>
/// Get an hash code for the vector
/// </summary>
/// <returns>The hash code</returns>
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) {

31
src/Vector3Float.cs
View File

@ -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

4
test/AngleTest.cs
View File

@ -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);

25
test/DirectionTest.cs
View File

@ -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);

186
test/SphericalTest.cs
View File

@ -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<Spherical> { 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<Spherical> {
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<Spherical> {
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