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

Squashed 'Assets/NanoBrain/LinearAlgebra/' changes from 672f8bf..a0bbb5b

Browse Source 
a0bbb5b Improve unit tests
f3044db Add more ToString
e64eaa2 Fix direction  To Unity Vector3
3bcd212 Fixed Spherical.ToVector3
a232385 Merge commit '841d923fed686700610a85aeab6289e44239aa6c'

git-subtree-dir: Assets/NanoBrain/LinearAlgebra
git-subtree-split: a0bbb5b5dd2e938be6d8a516bb69eb1130c4a51d
This commit is contained in:
Pascal Serrarens 2026-01-07 14:33:10 +01:00
parent 841d923fed
commit 1855aa0705
6 changed files with 107 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Angle.cs
View File

@ -57,7 +57,7 @@ namespace LinearAlgebra {
public readonly float inRevolutions => this.value / 360.0f; public readonly float inRevolutions => this.value / 360.0f;
public override string ToString() { public override string ToString() {
return $"{this.inDegrees} deg."; return $"{this.inDegrees}\u00B0";
} }
public static readonly AngleFloat zero = Degrees(0); public static readonly AngleFloat zero = Degrees(0);

50
src/Direction.cs
View File

@ -3,7 +3,8 @@ using System;
using Vector3Float = UnityEngine.Vector3; using Vector3Float = UnityEngine.Vector3;
#endif #endif
namespace LinearAlgebra { namespace LinearAlgebra
{
/// <summary> /// <summary>
/// A direction in 3D space /// A direction in 3D space
@ -16,7 +17,8 @@ namespace LinearAlgebra {
/// rotation has been applied. /// rotation has been applied.
/// The angles are automatically normalized to stay within the abovenmentioned /// The angles are automatically normalized to stay within the abovenmentioned
/// ranges. /// ranges.
public struct Direction { public struct Direction
{
/// @brief horizontal angle, range = (-180..180] degrees /// @brief horizontal angle, range = (-180..180] degrees
public AngleFloat horizontal; public AngleFloat horizontal;
/// @brief vertical angle, range in degrees = (-90..90] degrees /// @brief vertical angle, range in degrees = (-90..90] degrees
@ -29,7 +31,8 @@ namespace LinearAlgebra {
/// <param name="vertical">The vertical angle</param> /// <param name="vertical">The vertical angle</param>
/// <remarks>The direction will be normalized automatically /// <remarks>The direction will be normalized automatically
/// to ensure the angles are within the allowed ranges</remarks> /// to ensure the angles are within the allowed ranges</remarks>
public Direction(AngleFloat horizontal, AngleFloat vertical) { public Direction(AngleFloat horizontal, AngleFloat vertical)
{
this.horizontal = horizontal; this.horizontal = horizontal;
this.vertical = vertical; this.vertical = vertical;
this.Normalize(); this.Normalize();
@ -43,8 +46,10 @@ namespace LinearAlgebra {
/// <returns>The direction</returns> /// <returns>The direction</returns>
/// <remarks>The direction will be normalized automatically /// <remarks>The direction will be normalized automatically
/// to ensure the angles are within the allowed ranges</remarks> /// to ensure the angles are within the allowed ranges</remarks>
public static Direction Degrees(float horizontal, float vertical) { public static Direction Degrees(float horizontal, float vertical)
Direction d = new() { {
Direction d = new()
{
horizontal = AngleFloat.Degrees(horizontal), horizontal = AngleFloat.Degrees(horizontal),
vertical = AngleFloat.Degrees(vertical) vertical = AngleFloat.Degrees(vertical)
}; };
@ -57,8 +62,10 @@ namespace LinearAlgebra {
/// <param name="horizontal">The horizontal angle in radians</param> /// <param name="horizontal">The horizontal angle in radians</param>
/// <param name="vertical">The vertical angle in radians</param> /// <param name="vertical">The vertical angle in radians</param>
/// <returns>The direction</returns> /// <returns>The direction</returns>
public static Direction Radians(float horizontal, float vertical) { public static Direction Radians(float horizontal, float vertical)
Direction d = new() { {
Direction d = new()
{
horizontal = AngleFloat.Radians(horizontal), horizontal = AngleFloat.Radians(horizontal),
vertical = AngleFloat.Radians(vertical) vertical = AngleFloat.Radians(vertical)
}; };
@ -99,8 +106,10 @@ namespace LinearAlgebra {
/// </summary> /// </summary>
public readonly static Direction right = Degrees(90, 0); public readonly static Direction right = Degrees(90, 0);
private void Normalize() { private void Normalize()
if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) { {
if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90)
{
this.horizontal += AngleFloat.deg180; this.horizontal += AngleFloat.deg180;
this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees); this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees);
} }
@ -118,9 +127,11 @@ namespace LinearAlgebra {
// Calculate Vector // Calculate Vector
float cosV = MathF.Cos(radV); float cosV = MathF.Cos(radV);
float x = cosV * MathF.Cos(radH); float sinV = MathF.Sin(radV);
float y = MathF.Sin(radV);
float z = cosV * MathF.Sin(radH); float x = cosV * MathF.Sin(radH);
float y = sinV;
float z = cosV * MathF.Cos(radH);
return new UnityEngine.Vector3(x, y, z); return new UnityEngine.Vector3(x, y, z);
} }
@ -168,7 +179,8 @@ namespace LinearAlgebra {
/// <returns>The direction</returns> /// <returns>The direction</returns>
/// <remarks>Information about the length of the carthesian vector is not /// <remarks>Information about the length of the carthesian vector is not
/// included in this transformation</remarks> /// included in this transformation</remarks>
public static Direction FromVector3(Vector3Float v) { public static Direction FromVector3(Vector3Float v)
{
AngleFloat horizontal = AngleFloat.Atan2(v.horizontal, v.depth); AngleFloat horizontal = AngleFloat.Atan2(v.horizontal, v.depth);
AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.vertical); AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.vertical);
Direction d = new(horizontal, vertical); Direction d = new(horizontal, vertical);
@ -183,7 +195,8 @@ namespace LinearAlgebra {
/// <param name="d1"></param> /// <param name="d1"></param>
/// <param name="d2"></param> /// <param name="d2"></param>
/// <returns>True when the direction angles are equal, false otherwise.</returns> /// <returns>True when the direction angles are equal, false otherwise.</returns>
public static bool operator ==(Direction d1, Direction d2) { public static bool operator ==(Direction d1, Direction d2)
{
bool horizontalEq = d1.horizontal == d2.horizontal; bool horizontalEq = d1.horizontal == d2.horizontal;
bool verticalEq = d1.vertical == d2.vertical; bool verticalEq = d1.vertical == d2.vertical;
return horizontalEq && verticalEq; return horizontalEq && verticalEq;
@ -195,13 +208,15 @@ namespace LinearAlgebra {
/// <param name="d1"></param> /// <param name="d1"></param>
/// <param name="d2"></param> /// <param name="d2"></param>
/// <returns>True when the direction angles are not equal, false otherwise.</returns> /// <returns>True when the direction angles are not equal, false otherwise.</returns>
public static bool operator !=(Direction d1, Direction d2) { public static bool operator !=(Direction d1, Direction d2)
{
bool horizontalNEq = d1.horizontal != d2.horizontal; bool horizontalNEq = d1.horizontal != d2.horizontal;
bool verticalNEq = d1.vertical != d2.vertical; bool verticalNEq = d1.vertical != d2.vertical;
return horizontalNEq || verticalNEq; return horizontalNEq || verticalNEq;
} }
public override readonly bool Equals(object obj) { public override readonly bool Equals(object obj)
{
if (obj is not Direction d) if (obj is not Direction d)
return false; return false;
@ -211,7 +226,8 @@ namespace LinearAlgebra {
} }
public override readonly int GetHashCode() { public override readonly int GetHashCode()
{
return HashCode.Combine(horizontal, vertical); return HashCode.Combine(horizontal, vertical);
} }

75
src/Spherical.cs
View File

@ -62,28 +62,34 @@ namespace LinearAlgebra {
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
public static Spherical FromVector3(Vector3 v) { public static Spherical FromVector3(Vector3 v) {
float distance = v.magnitude; float distance = v.magnitude;
if (distance == 0.0f) // if (distance == 0.0f)
return Spherical.zero; // return Spherical.zero;
else { // else {
float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * AngleFloat.Rad2Deg; // float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * AngleFloat.Rad2Deg;
float horizontalAngle = (float)Math.Atan2(v.x, v.z) * AngleFloat.Rad2Deg; // float horizontalAngle = (float)Math.Atan2(v.x, v.z) * AngleFloat.Rad2Deg;
return Degrees(distance, horizontalAngle, verticalAngle); // return Degrees(distance, horizontalAngle, verticalAngle);
} // }
Direction direction = Direction.FromVector3(v.normalized);
return new Spherical(distance, direction);
} }
public readonly Vector3 ToVector3() { public readonly Vector3 ToVector3() {
float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians; // float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
float horizontalRad = this.direction.horizontal.inRadians; // float horizontalRad = this.direction.horizontal.inRadians;
float cosVertical = (float)Math.Cos(verticalRad); // float cosVertical = (float)Math.Cos(verticalRad);
float sinVertical = (float)Math.Sin(verticalRad); // float sinVertical = (float)Math.Sin(verticalRad);
float cosHorizontal = (float)Math.Cos(horizontalRad); // float cosHorizontal = (float)Math.Cos(horizontalRad);
float sinHorizontal = (float)Math.Sin(horizontalRad); // float sinHorizontal = (float)Math.Sin(horizontalRad);
float x = this.distance * sinVertical * sinHorizontal; // float x = this.distance * sinVertical * sinHorizontal;
float y = this.distance * cosVertical; // float y = this.distance * cosVertical;
float z = this.distance * sinVertical * cosHorizontal; // float z = this.distance * sinVertical * cosHorizontal;
Vector3 v = new(x, y, z); // Vector3 v = new(x, y, z);
// return v;
Vector3 v = this.direction.ToVector3();
v *= this.distance;
return v; return v;
} }
#else #else
@ -99,22 +105,29 @@ namespace LinearAlgebra {
} }
public readonly Vector3Float ToVector3() { public readonly Vector3Float ToVector3() {
float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians; // float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
float horizontalRad = this.direction.horizontal.inRadians; // float horizontalRad = this.direction.horizontal.inRadians;
float cosVertical = (float)Math.Cos(verticalRad); // float cosVertical = (float)Math.Cos(verticalRad);
float sinVertical = (float)Math.Sin(verticalRad); // float sinVertical = (float)Math.Sin(verticalRad);
float cosHorizontal = (float)Math.Cos(horizontalRad); // float cosHorizontal = (float)Math.Cos(horizontalRad);
float sinHorizontal = (float)Math.Sin(horizontalRad); // float sinHorizontal = (float)Math.Sin(horizontalRad);
float x = this.distance * sinVertical * sinHorizontal; // float x = this.distance * sinVertical * sinHorizontal;
float y = this.distance * cosVertical; // float y = this.distance * cosVertical;
float z = this.distance * sinVertical * cosHorizontal; // float z = this.distance * sinVertical * cosHorizontal;
Vector3Float v = new(x, y, z); // Vector3Float v = new(x, y, z);
Vector3Float v = this.direction.ToVector3();
v *= this.distance;
return v; return v;
} }
#endif #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 readonly float magnitude => this.distance;
public Spherical normalized { public Spherical normalized {
@ -228,7 +241,7 @@ namespace LinearAlgebra {
return Spherical.Radians(rAvg, azAvgRad, elAvgRad); return Spherical.Radians(rAvg, azAvgRad, elAvgRad);
} }
/*
public static Spherical Average(List<Spherical> vectors) { public static Spherical Average(List<Spherical> vectors) {
// float sumSinPhiCosTheta = 0.0f; // float sumSinPhiCosTheta = 0.0f;
// float sumSinPhiSinTheta = 0.0f; // float sumSinPhiSinTheta = 0.0f;
@ -302,9 +315,9 @@ namespace LinearAlgebra {
return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg)); return new Spherical(rAvg, new Direction(horizontalAvg, verticalAvg));
} }
*/
/*
public static Spherical Average(IEnumerable<Spherical> vectors) { public static Spherical Average(IEnumerable<Spherical> vectors) {
const float EPS = 1e-6f; const float EPS = 1e-6f;
if (vectors == null) throw new ArgumentNullException(nameof(vectors)); if (vectors == null) throw new ArgumentNullException(nameof(vectors));
@ -371,7 +384,7 @@ namespace LinearAlgebra {
return Spherical.Radians(rAvgFinal, azAvg, elAvg); return Spherical.Radians(rAvgFinal, azAvg, elAvg);
} }
*/
} }
} }

4
src/Vector3Float.cs
View File

@ -119,6 +119,10 @@ namespace LinearAlgebra {
return new Vector3Float(horizontal, vertical, depth); return new Vector3Float(horizontal, vertical, depth);
} }
public override string ToString() {
return $"({this.horizontal}, {this.vertical}, {this.depth})";
}
/// <summary> /// <summary>
/// A vector with zero for all axis /// A vector with zero for all axis
/// </summary> /// </summary>

2
test/DirectionTest.cs
View File

@ -103,7 +103,7 @@ namespace LinearAlgebra.Test {
[Test] [Test]
public void ToVector3AndBack2() { public void ToVector3AndBack2() {
Direction d1 = Direction.Degrees(135, 85); Direction d1 = Direction.Degrees(-135, 85);
Vector3Float v = d1.ToVector3(); Vector3Float v = d1.ToVector3();
Direction d2 = Direction.FromVector3(v); Direction d2 = Direction.FromVector3(v);
Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f); Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f);

24
test/SphericalTest.cs
View File

@ -234,6 +234,30 @@ namespace LinearAlgebra.Test {
Assert.AreEqual(MathF.Cos(alpha), avg.distance, 1e-4f); Assert.AreEqual(MathF.Cos(alpha), avg.distance, 1e-4f);
} }
[Test]
public void Average_CompareWithVector3() {
// 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
List<Spherical> dirs = new List<Spherical> {
new(1f, Direction.Radians(0f, elevation)),
new(2f, Direction.Radians(MathF.PI/2, elevation+1)),
new(3f, Direction.Radians(MathF.PI, elevation+2)),
new(4f, Direction.Radians(3*MathF.PI/2, elevation+3))
};
Spherical avg = Spherical.Average(dirs);
Vector3Float r = Vector3Float.zero;
foreach (Spherical dir in dirs) {
r += dir.ToVector3();
}
r = r / 4;
Spherical avg2 = Spherical.FromVector3(r);
Assert.AreEqual(avg, avg2);
}
} }
} }
#endif #endif