RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Migrated to RoboidControl

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-12 14:52:14 +01:00
commit 7ecc78ee55
15 changed files with 2307 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored Normal file
View File

@ -0,0 +1,6 @@
DoxyGen/DoxyWarnLogfile.txt
.vscode/settings.json
**bin
**obj
**.meta
*.sln

117
src/Angle.cs Normal file
View File

@ -0,0 +1,117 @@
using System;
namespace LinearAlgebra
{
/// <summary>
/// %Angle utilities
/// </summary>
public static class Angle
{
public const float pi = 3.1415927410125732421875F;
// public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
// public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;
public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F;
/// <summary>
/// Clamp the angle between the given min and max values
/// </summary>
/// <param name="angle">The angle to clamp</param>
/// <param name="min">The minimum angle</param>
/// <param name="max">The maximum angle</param>
/// <returns>The clamped angle</returns>
/// Angles are normalized
public static float Clamp(float angle, float min, float max)
{
float normalizedAngle = Normalize(angle);
return Float.Clamp(normalizedAngle, min, max);
}
/// <summary>
/// Determine the angle difference, result is a normalized angle
/// </summary>
/// <param name="a">First first angle</param>
/// <param name="b">The second angle</param>
/// <returns>the angle between the two angles</returns>
/// Angle values should be degrees
public static float Difference(float a, float b)
{
float r = Normalize(b - a);
return r;
}
/// <summary>
/// Normalize an angle to the range -180 < angle <= 180
/// </summary>
/// <param name="angle">The angle to normalize</param>
/// <returns>The normalized angle in interval (-180..180] </returns>
/// Angle values should be in degrees
public static float Normalize(float angle)
{
if (float.IsInfinity(angle))
return angle;
while (angle <= -180) angle += 360;
while (angle > 180) angle -= 360;
return angle;
}
/// <summary>
/// Rotate from one angle to the other with a maximum degrees
/// </summary>
/// <param name="fromAngle">Starting angle</param>
/// <param name="toAngle">Target angle</param>
/// <param name="maxAngle">Maximum angle to rotate</param>
/// <returns>The resulting angle</returns>
/// This function is compatible with radian and degrees angles
public static float MoveTowards(float fromAngle, float toAngle, float maxAngle)
{
float d = toAngle - fromAngle;
d = Normalize(d);
d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle);
return fromAngle + d;
}
/// <summary>
/// Map interval of angles between vectors [0..Pi] to interval [0..1]
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The resulting factor in interval [0..1]</returns>
/// Vectors a and b must be normalized
/// \deprecated Please use Vector2.ToFactor instead.
[Obsolete("Please use Vector2.ToFactor instead.")]
public static float ToFactor(Vector2 v1, Vector2 v2)
{
return (1 - Vector2.Dot(v1, v2)) / 2;
}
// Normalize all vector angles to the range -180 < angle < 180
//public static Vector3 Normalize(Vector3 angles) {
// float x = Normalize(angles.x);
// float y = Normalize(angles.y);
// float z = Normalize(angles.z);
// return new Vector3(x, y, z);
//}
// Returns the signed angle in degrees between from and to.
//public static float SignedAngle(Vector3 from, Vector3 to) {
// float angle = Vector3.Angle(from, to);
// Vector3 cross = Vector3.Cross(from, to);
// if (cross.y < 0) angle = -angle;
// return angle;
//}
// Returns the signed angle in degrees between from and to.
//public static float SignedAngle(Vector2 from, Vector2 to) {
// float sign = Math.Sign(from.y * to.x - from.x * to.y);
// return Vector2.Angle(from, to) * sign;
//}
//public static Quaternion ToQuaternion(Rotation orientation) {
// return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w);
//}
}
}

60
src/Direction.cs Normal file
View File

@ -0,0 +1,60 @@
using System;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
#endif
namespace LinearAlgebra
{
public class Direction
{
public float horizontal;
public float vertical;
public Direction()
{
horizontal = 0;
vertical = 0;
}
public Direction(float horizontal, float vertical)
{
this.horizontal = horizontal;
this.vertical = vertical;
//Normalize();
}
public readonly static Direction forward = new Direction(0, 0);
public readonly static Direction backward = new Direction(-180, 0);
public readonly static Direction up = new Direction(0, 90);
public readonly static Direction down = new Direction(0, -90);
public readonly static Direction left = new Direction(-90, 0);
public readonly static Direction right = new Direction(90, 0);
public void Normalize()
{
if (this.vertical > 90 || this.vertical < -90)
{
this.horizontal += 180;
this.vertical = 180 - this.vertical;
}
}
public Vector3Float ToVector3()
{
float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad;
float horizontalRad = this.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 = sinVertical * sinHorizontal;
float y = cosVertical;
float z = sinVertical * cosHorizontal;
Vector3Float v = new(x, y, z);
return v;
}
}
}

45
src/Float.cs Normal file
View File

@ -0,0 +1,45 @@
namespace LinearAlgebra
{
/// <summary>
/// Float number utilities
/// </summary>
public class Float
{
/// <summary>
/// The precision of float numbers
/// </summary>
public const float epsilon = 1E-05f;
/// <summary>
/// The square of the float number precision
/// </summary>
public const float sqrEpsilon = 1e-10f;
/// <summary>
/// Clamp the value between the given minimum and maximum values
/// </summary>
/// <param name="f">The value to clamp</param>
/// <param name="min">The minimum value</param>
/// <param name="max">The maximum value</param>
/// <returns>The clamped value</returns>
public static float Clamp(float f, float min, float max)
{
if (f < min)
return min;
if (f > max)
return max;
return f;
}
/// <summary>
/// Clamp the value between to the interval [0..1]
/// </summary>
/// <param name="f">The value to clamp</param>
/// <returns>The clamped value</returns>
public static float Clamp01(float f)
{
return Clamp(f, 0, 1);
}
}
}

14
src/LinearAlgebra.csproj Normal file
View File

@ -0,0 +1,14 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<GenerateAssemblyInfo>false</GenerateAssemblyInfo>
<GenerateTargetFrameworkAttribute>false</GenerateTargetFrameworkAttribute>
<TargetFramework>net5.0</TargetFramework>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
<PackageReference Include="NUnit" Version="3.13.2" />
</ItemGroup>
</Project>

645
src/Matrix.cs Normal file
View File

@ -0,0 +1,645 @@
using System;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
using Vector2Float = UnityEngine.Vector2;
using Quaternion = UnityEngine.Quaternion;
#endif
namespace LinearAlgebra
{
public readonly struct Slice
{
public uint start { get; }
public uint stop { get; }
public Slice(uint start, uint stop)
{
this.start = start;
this.stop = stop;
}
}
public class Matrix2
{
public float[,] data { get; }
public uint nRows => (uint)data.GetLength(0);
public uint nCols => (uint)data.GetLength(1);
public Matrix2(uint nRows, uint nCols)
{
this.data = new float[nRows, nCols];
}
public Matrix2(float[,] data)
{
this.data = data;
}
public Matrix2 Clone()
{
float[,] data = new float[this.nRows, nCols];
for (int rowIx = 0; rowIx < this.nRows; rowIx++)
{
for (int colIx = 0; colIx < this.nCols; colIx++)
data[rowIx, colIx] = this.data[rowIx, colIx];
}
return new Matrix2(data);
}
public static Matrix2 Zero(uint nRows, uint nCols)
{
return new Matrix2(nRows, nCols);
}
public static Matrix2 FromVector3(Vector3Float v)
{
float[,] result = new float[3, 1];
result[0, 0] = v.x;
result[1, 0] = v.y;
result[2, 0] = v.z;
return new Matrix2(result);
}
public static Matrix2 Identity(uint size)
{
return Diagonal(1, size);
}
public static Matrix2 Identity(uint nRows, uint nCols)
{
Matrix2 m = Zero(nRows, nCols);
m.FillDiagonal(1);
return m;
}
public static Matrix2 Diagonal(Matrix1 v)
{
float[,] resultData = new float[v.size, v.size];
for (int ix = 0; ix < v.size; ix++)
resultData[ix, ix] = v.data[ix];
return new Matrix2(resultData);
}
public static Matrix2 Diagonal(float f, uint size)
{
float[,] resultData = new float[size, size];
for (int ix = 0; ix < size; ix++)
resultData[ix, ix] = f;
return new Matrix2(resultData);
}
public void FillDiagonal(Matrix1 v)
{
uint n = Math.Min(Math.Min(this.nRows, this.nCols), v.size);
for (int ix = 0; ix < n; ix++)
this.data[ix, ix] = v.data[ix];
}
public void FillDiagonal(float f)
{
uint n = Math.Min(this.nRows, this.nCols);
for (int ix = 0; ix < n; ix++)
this.data[ix, ix] = f;
}
public static Matrix2 SkewMatrix(Vector3Float v)
{
float[,] result = new float[3, 3] {
{0, -v.z, v.y},
{v.z, 0, -v.x},
{-v.y, v.x, 0}
};
return new Matrix2(result);
}
#if UNITY_5_3_OR_NEWER
public Vector3Float GetRow3(int rowIx) {
uint cols = this.nCols;
Vector3Float row = new() {
x = this.data[rowIx, 0],
y = this.data[rowIx, 1],
z = this.data[rowIx, 2]
};
return row;
}
#endif
public void SetRow(int rowIx, Matrix1 v)
{
for (uint ix = 0; ix < v.size; ix++)
this.data[rowIx, ix] = v.data[ix];
}
public void SetRow3(int rowIx, Vector3Float v)
{
this.data[rowIx, 0] = v.x;
this.data[rowIx, 1] = v.y;
this.data[rowIx, 2] = v.z;
}
public Matrix1 GetColumn(int colIx)
{
float[] column = new float[this.nRows];
for (int i = 0; i < this.nRows; i++)
{
column[i] = this.data[i, colIx];
}
return new Matrix1(column);
}
public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f)
{
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
{
float d = MathF.Abs(A.data[i, j] - B.data[i, j]);
if (d > atol)
return false;
}
}
return true;
}
public Matrix2 Transpose()
{
float[,] resultData = new float[this.nCols, this.nRows];
for (uint rowIx = 0; rowIx < this.nRows; rowIx++)
{
for (uint colIx = 0; colIx < this.nCols; colIx++)
resultData[colIx, rowIx] = this.data[rowIx, colIx];
}
return new Matrix2(resultData);
// double checked code
}
public Matrix2 transposed
{
get => Transpose();
}
public static Matrix2 operator -(Matrix2 m)
{
float[,] result = new float[m.nRows, m.nCols];
for (int i = 0; i < m.nRows; i++)
{
for (int j = 0; j < m.nCols; j++)
result[i, j] = -m.data[i, j];
}
return new Matrix2(result);
}
public static Matrix2 operator -(Matrix2 A, Matrix2 B)
{
if (A.nRows != B.nRows || A.nCols != B.nCols)
throw new System.ArgumentException("Size of A must match size of B.");
float[,] result = new float[A.nRows, B.nCols];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
result[i, j] = A.data[i, j] - B.data[i, j];
}
return new Matrix2(result);
}
public static Matrix2 operator +(Matrix2 A, Matrix2 B)
{
if (A.nRows != B.nRows || A.nCols != B.nCols)
throw new System.ArgumentException("Size of A must match size of B.");
float[,] result = new float[A.nRows, B.nCols];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
result[i, j] = A.data[i, j] + B.data[i, j];
}
return new Matrix2(result);
}
public static Matrix2 operator *(Matrix2 A, Matrix2 B)
{
if (A.nCols != B.nRows)
throw new System.ArgumentException("Number of columns in A must match number of rows in B.");
float[,] result = new float[A.nRows, B.nCols];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < B.nCols; j++)
{
float sum = 0.0f;
for (int k = 0; k < A.nCols; k++)
sum += A.data[i, k] * B.data[k, j];
result[i, j] = sum;
}
}
return new Matrix2(result);
// double checked code
}
public static Matrix1 operator *(Matrix2 A, Matrix1 v)
{
float[] result = new float[A.nRows];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
{
result[i] += A.data[i, j] * v.data[j];
}
}
return new Matrix1(result);
}
public static Vector3Float operator *(Matrix2 A, Vector3Float v)
{
return new Vector3Float(
A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
);
}
public static Matrix2 operator *(Matrix2 A, float s)
{
float[,] result = new float[A.nRows, A.nCols];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
result[i, j] = A.data[i, j] * s;
}
return new Matrix2(result);
}
public static Matrix2 operator *(float s, Matrix2 A)
{
return A * s;
}
public static Matrix2 operator /(Matrix2 A, float s)
{
float[,] result = new float[A.nRows, A.nCols];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
result[i, j] = A.data[i, j] / s;
}
return new Matrix2(result);
}
public static Matrix2 operator /(float s, Matrix2 A)
{
float[,] result = new float[A.nRows, A.nCols];
for (int i = 0; i < A.nRows; i++)
{
for (int j = 0; j < A.nCols; j++)
result[i, j] = s / A.data[i, j];
}
return new Matrix2(result);
}
public Matrix2 Slice(Slice slice)
{
return Slice(slice.start, slice.stop);
}
public Matrix2 Slice(uint from, uint to)
{
if (from < 0 || to >= this.nRows)
throw new System.ArgumentException("Slice index out of range.");
float[,] result = new float[to - from, this.nCols];
int resultRowIx = 0;
for (uint rowIx = from; rowIx < to; rowIx++)
{
for (int colIx = 0; colIx < this.nCols; colIx++)
{
result[resultRowIx, colIx] = this.data[rowIx, colIx];
}
resultRowIx++;
}
return new Matrix2(result);
}
public Matrix2 Slice(Slice rowRange, Slice colRange)
{
return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
}
public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange)
{
float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
uint resultRowIx = 0;
uint resultColIx = 0;
for (uint i = rowRange.start; i < rowRange.stop; i++)
{
for (uint j = colRange.start; j < colRange.stop; j++)
result[resultRowIx, resultColIx] = this.data[i, j];
}
return new Matrix2(result);
}
public void UpdateSlice(Slice slice, Matrix2 m)
{
int mRowIx = 0;
for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++)
{
for (int colIx = 0; colIx < this.nCols; colIx++)
this.data[rowIx, colIx] = m.data[mRowIx, colIx];
}
}
public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m)
{
UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
}
public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m)
{
for (uint i = rowRange.start; i < rowRange.stop; i++)
{
for (uint j = colRange.start; j < colRange.stop; j++)
this.data[i, j] = m.data[i - rowRange.start, j - colRange.start];
}
}
public Matrix2 Inverse()
{
Matrix2 A = this;
// unchecked
uint n = A.nRows;
// Create an identity matrix of the same size as the original matrix
float[,] augmentedMatrix = new float[n, 2 * n];
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
augmentedMatrix[i, j] = A.data[i, j];
augmentedMatrix[i, j + n] = (i == j) ? 1 : 0; // Identity matrix
}
}
// Perform Gaussian elimination
for (int i = 0; i < n; i++)
{
// Find the pivot row
float pivot = augmentedMatrix[i, i];
if (Math.Abs(pivot) < 1e-10) // Check for singular matrix
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
// Normalize the pivot row
for (int j = 0; j < 2 * n; j++)
augmentedMatrix[i, j] /= pivot;
// Eliminate the column below the pivot
for (int j = i + 1; j < n; j++)
{
float factor = augmentedMatrix[j, i];
for (int k = 0; k < 2 * n; k++)
augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
}
}
// Back substitution
for (uint i = n - 1; i >= 0; i--)
{
// Eliminate the column above the pivot
for (uint j = i - 1; j >= 0; j--)
{
float factor = augmentedMatrix[j, i];
for (int k = 0; k < 2 * n; k++)
augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
}
}
// Extract the inverse matrix from the augmented matrix
float[,] inverse = new float[n, n];
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
inverse[i, j] = augmentedMatrix[i, j + n];
}
return new Matrix2(inverse);
}
public float Determinant()
{
uint n = this.nRows;
if (n != this.nCols)
throw new System.ArgumentException("Matrix must be square.");
if (n == 1)
return this.data[0, 0]; // Base case for 1x1 matrix
if (n == 2) // Base case for 2x2 matrix
return this.data[0, 0] * this.data[1, 1] - this.data[0, 1] * this.data[1, 0];
float det = 0;
for (int col = 0; col < n; col++)
det += (col % 2 == 0 ? 1 : -1) * this.data[0, col] * this.Minor(0, col).Determinant();
return det;
}
// Helper function to compute the minor of a matrix
private Matrix2 Minor(int rowToRemove, int colToRemove)
{
uint n = this.nRows;
float[,] minor = new float[n - 1, n - 1];
int r = 0, c = 0;
for (int i = 0; i < n; i++)
{
if (i == rowToRemove) continue;
c = 0;
for (int j = 0; j < n; j++)
{
if (j == colToRemove) continue;
minor[r, c] = this.data[i, j];
c++;
}
r++;
}
return new Matrix2(minor);
}
}
public class Matrix1
{
public float[] data { get; }
public uint size => (uint)data.GetLength(0);
public Matrix1(uint size)
{
this.data = new float[size];
}
public Matrix1(float[] data)
{
this.data = data;
}
public static Matrix1 Zero(uint size)
{
return new Matrix1(size);
}
public static Matrix1 FromVector2(Vector2Float v)
{
float[] result = new float[2];
result[0] = v.x;
result[1] = v.y;
return new Matrix1(result);
}
public static Matrix1 FromVector3(Vector3Float v)
{
float[] result = new float[3];
result[0] = v.x;
result[1] = v.y;
result[2] = v.z;
return new Matrix1(result);
}
#if UNITY_5_3_OR_NEWER
public static Matrix1 FromQuaternion(Quaternion q) {
float[] result = new float[4];
result[0] = q.x;
result[1] = q.y;
result[2] = q.z;
result[3] = q.w;
return new Matrix1(result);
}
#endif
public Vector2Float vector2
{
get
{
if (this.size != 2)
throw new System.ArgumentException("Matrix1 must be of size 2");
return new Vector2Float(this.data[0], this.data[1]);
}
}
public Vector3Float vector3
{
get
{
if (this.size != 3)
throw new System.ArgumentException("Matrix1 must be of size 3");
return new Vector3Float(this.data[0], this.data[1], this.data[2]);
}
}
#if UNITY_5_3_OR_NEWER
public Quaternion quaternion {
get {
if (this.size != 4)
throw new System.ArgumentException("Matrix1 must be of size 4");
return new Quaternion(this.data[0], this.data[1], this.data[2], this.data[3]);
}
}
#endif
public Matrix1 Clone()
{
float[] data = new float[this.size];
for (int rowIx = 0; rowIx < this.size; rowIx++)
data[rowIx] = this.data[rowIx];
return new Matrix1(data);
}
public float magnitude
{
get
{
float sum = 0;
foreach (var elm in data)
sum += elm;
return sum / data.Length;
}
}
public static Matrix1 operator +(Matrix1 A, Matrix1 B)
{
if (A.size != B.size)
throw new System.ArgumentException("Size of A must match size of B.");
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
{
result[i] = A.data[i] + B.data[i];
}
return new Matrix1(result);
}
public Matrix2 Transpose()
{
float[,] r = new float[1, this.size];
for (uint colIx = 0; colIx < this.size; colIx++)
r[1, colIx] = this.data[colIx];
return new Matrix2(r);
}
public static float Dot(Matrix1 a, Matrix1 b)
{
if (a.size != b.size)
throw new System.ArgumentException("Vectors must be of the same length.");
float result = 0.0f;
for (int i = 0; i < a.size; i++)
{
result += a.data[i] * b.data[i];
}
return result;
}
public static Matrix1 operator *(Matrix1 A, float f)
{
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
result[i] += A.data[i] * f;
return new Matrix1(result);
}
public static Matrix1 operator *(float f, Matrix1 A)
{
return A * f;
}
public Matrix1 Slice(Slice range)
{
return Slice(range.start, range.stop);
}
public Matrix1 Slice(uint from, uint to)
{
if (from < 0 || to >= this.size)
throw new System.ArgumentException("Slice index out of range.");
float[] result = new float[to - from];
int resultIx = 0;
for (uint ix = from; ix < to; ix++)
result[resultIx++] = this.data[ix];
return new Matrix1(result);
}
public void UpdateSlice(Slice slice, Matrix1 v)
{
int vIx = 0;
for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
this.data[ix] = v.data[vIx];
}
}
}

98
src/Quat32.cs Normal file
View File

@ -0,0 +1,98 @@
using System;
namespace LinearAlgebra
{
public class Quat32
{
public float x;
public float y;
public float z;
public float w;
public Quat32()
{
this.x = 0;
this.y = 0;
this.z = 0;
this.w = 1;
}
public Quat32(float x, float y, float z, float w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public static Quat32 FromSwingTwist(SwingTwist s)
{
Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
return q32;
}
public static Quat32 Euler(float yaw, float pitch, float roll)
{
float rollOver2 = roll * Angle.Deg2Rad * 0.5f;
float sinRollOver2 = (float)Math.Sin((float)rollOver2);
float cosRollOver2 = (float)Math.Cos((float)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)Math.Sin((float)pitchOver2);
float cosPitchOver2 = (float)Math.Cos((float)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)Math.Sin((float)yawOver2);
float cosYawOver2 = (float)Math.Cos((float)yawOver2);
Quat32 result = new Quat32()
{
w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
sinYawOver2 * sinPitchOver2 * sinRollOver2,
x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
cosYawOver2 * sinPitchOver2 * sinRollOver2,
y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
sinYawOver2 * cosPitchOver2 * sinRollOver2,
z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
sinYawOver2 * sinPitchOver2 * cosRollOver2
};
return result;
}
public void ToAngles(out float right, out float up, out float forward)
{
float test = this.x * this.y + this.z * this.w;
if (test > 0.499f)
{ // singularity at north pole
right = 0;
up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
forward = 90;
return;
//return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90);
}
else if (test < -0.499f)
{ // singularity at south pole
right = 0;
up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
forward = -90;
return;
//return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90);
}
else
{
float sqx = this.x * this.x;
float sqy = this.y * this.y;
float sqz = this.z * this.z;
right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg;
up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg;
forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg;
return;
// return Vector3(
// atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) *
// Rad2Deg,
// atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) *
// Rad2Deg,
// asinf(2 * test) * Angle.Rad2Deg);
}
}
}
}

80
src/Quaternion.cs Normal file
View File

@ -0,0 +1,80 @@
using System;
#if UNITY_5_3_OR_NEWER
using Quaternion = UnityEngine.Quaternion;
#endif
namespace LinearAlgebra
{
public class QuaternionOf<T>
{
public T x;
public T y;
public T z;
public T w;
public QuaternionOf(T x, T y, T z, T w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
#if UNITY_5_3_OR_NEWER
public static Matrix2 ToRotationMatrix(Quaternion q) {
float w = q.x, x = q.y, y = q.z, z = q.w;
float[,] result = new float[,]
{
{ 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) },
{ 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) },
{ 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }
};
return new Matrix2(result);
}
public static Quaternion FromRotationMatrix(Matrix2 m) {
float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2];
float w, x, y, z;
if (trace > 0) {
float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
w = 0.25f / s;
x = (m.data[2, 1] - m.data[1, 2]) * s;
y = (m.data[0, 2] - m.data[2, 0]) * s;
z = (m.data[1, 0] - m.data[0, 1]) * s;
}
else {
if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) {
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]);
w = (m.data[2, 1] - m.data[1, 2]) / s;
x = 0.25f * s;
y = (m.data[0, 1] + m.data[1, 0]) / s;
z = (m.data[0, 2] + m.data[2, 0]) / s;
}
else if (m.data[1, 1] > m.data[2, 2]) {
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]);
w = (m.data[0, 2] - m.data[2, 0]) / s;
x = (m.data[0, 1] + m.data[1, 0]) / s;
y = 0.25f * s;
z = (m.data[1, 2] + m.data[2, 1]) / s;
}
else {
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]);
w = (m.data[1, 0] - m.data[0, 1]) / s;
x = (m.data[0, 2] + m.data[2, 0]) / s;
y = (m.data[1, 2] + m.data[2, 1]) / s;
z = 0.25f * s;
}
}
return new Quaternion(x, y, z, w);
}
#endif
}
// public class Quaternion : QuaternionOf<float> {
// public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
// }
}

57
src/Spherical.cs Normal file
View File

@ -0,0 +1,57 @@
using System;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
#endif
namespace LinearAlgebra
{
public class Spherical
{
public float distance;
public Direction direction;
public static Spherical zero = new Spherical(0, 0, 0);
public static Spherical forward = new Spherical(1, 0, 0);
public Spherical(float distance, float horizontal, float vertical)
{
this.distance = distance;
this.direction = new Direction(horizontal, vertical);
}
public Spherical(float distance, Direction direction)
{
this.distance = distance;
this.direction = direction;
}
public static Spherical FromVector3(Vector3Float v)
{
float distance = v.magnitude;
if (distance == 0.0f)
return new Spherical(distance, 0, 0);
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 new Spherical(distance, horizontalAngle, verticalAngle);
}
}
public Vector3Float ToVector3()
{
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 Vector3Float(x, y, z);
return v;
}
}
}

46
src/SwingTwist.cs Normal file
View File

@ -0,0 +1,46 @@
using System.Numerics;
#if UNITY_5_3_OR_NEWER
using Quaternion = UnityEngine.Quaternion;
#endif
namespace LinearAlgebra
{
public class SwingTwist
{
public Direction swing;
public float twist;
public static readonly SwingTwist zero = new SwingTwist(0, 0, 0);
public SwingTwist(Direction swing, float twist)
{
this.swing = swing;
this.twist = twist;
}
public SwingTwist(float horizontalSwing, float verticalSwing, float twist)
{
this.swing = new Direction(horizontalSwing, verticalSwing);
this.swing.Normalize();
this.twist = twist;
}
public static SwingTwist FromQuat32(Quat32 q32)
{
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
q32.ToAngles(out float right, out float up, out float forward);
SwingTwist r = new SwingTwist(up, right, forward);
return r;
}
#if UNITY_5_3_OR_NEWER
public Quaternion ToQuaternion() {
Quaternion q = Quaternion.Euler(-this.swing.vertical,
this.swing.horizontal,
this.twist);
return q;
}
#endif
}
}

401
src/Vector2.cs Normal file
View File

@ -0,0 +1,401 @@
using System;
using System.Numerics;
namespace LinearAlgebra
{
public class Vector2Of<T> where T : IComparable<T>
{
public T x;
public T y;
public Vector2Of(T x, T y)
{
this.x = x;
this.y = y;
}
}
public class Vector2Int : Vector2Of<int>
{
public Vector2Int(int x, int y) : base(x, y) { }
public static Vector2Int operator -(Vector2Int v1, Vector2Int v2)
{
return new Vector2Int(v1.x - v2.x, v1.y - v2.y);
}
public float magnitude
{
get
{
return (float)Math.Sqrt(this.x * this.x + this.y * this.y);
}
}
public static float Distance(Vector2Int v1, Vector2Int v2)
{
return (v1 - v2).magnitude;
}
}
public class Vector2Float : Vector2Of<float>
{
public Vector2Float(float x, float y) : base(x, y) { }
public static Vector2Float operator -(Vector2Float v1, Vector2Float v2)
{
return new Vector2Float(v1.x - v2.x, v1.y - v2.y);
}
public float magnitude
{
get
{
return (float)Math.Sqrt(this.x * this.x + this.y * this.y);
}
}
public static float Distance(Vector2Float v1, Vector2Float v2)
{
return (v1 - v2).magnitude;
}
}
/// <summary>
/// 2-dimensional vectors
/// </summary>
public struct Vector2 : IEquatable<Vector2>
{
/// <summary>
/// The right axis of the vector
/// </summary>
public float x; // left/right
/// <summary>
/// The upward/forward axis of the vector
/// </summary>
public float y; // forward/backward
// directions are to be inline with Vector3 as much as possible...
/// <summary>
/// Create a new 2-dimensional vector
/// </summary>
/// <param name="x">x axis value</param>
/// <param name="y">y axis value</param>
public Vector2(float x, float y)
{
this.x = x;
this.y = y;
}
/// <summary>
/// A vector with zero for all axis
/// </summary>
public static readonly Vector2 zero = new Vector2(0, 0);
/// <summary>
/// A vector with values (1, 1)
/// </summary>
public static readonly Vector2 one = new Vector2(1, 1);
/// <summary>
/// A vector with values (0, 1)
/// </summary>
public static readonly Vector2 up = new Vector2(0, 1);
/// <summary>
/// A vector with values (0, -1)
/// </summary>
public static readonly Vector2 down = new Vector2(0, -1);
/// <summary>
/// A vector with values (0, 1)
/// </summary>
public static readonly Vector2 forward = new Vector2(0, 1);
/// <summary>
/// A vector with values (0, -1)
/// </summary>
public static readonly Vector2 back = new Vector2(0, -1);
/// <summary>
/// A vector3 with values (-1, 0)
/// </summary>
public static readonly Vector2 left = new Vector2(-1, 0);
/// <summary>
/// A vector with values (1, 0)
/// </summary>
public static readonly Vector2 right = new Vector2(1, 0);
/// <summary>
/// The squared length of this vector
/// </summary>
/// <returns>The squared length</returns>
/// The squared length is computationally simpler than the real length.
/// Think of Pythagoras A^2 + B^2 = C^2.
/// This leaves out the calculation of the squared root of C.
public float sqrMagnitude
{
get
{
float d = x * x + y * y;
return d;
}
}
/// <summary>
/// The length of this vector
/// </summary>
/// <returns>The length of this vector</returns>
public float magnitude
{
get
{
float d = (float)Math.Sqrt(x * x + y * y);
return d;
}
}
/// <summary>
/// Convert the vector to a length of a 1
/// </summary>
/// <returns>The vector with length 1</returns>
public Vector2 normalized
{
get
{
float l = magnitude;
Vector2 v = zero;
if (l > Float.epsilon)
v = this / l;
return v;
}
}
/// <summary>
/// Add two vectors
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The result of adding the two vectors</returns>
public static Vector2 operator +(Vector2 v1, Vector2 v2)
{
Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y);
return v;
}
/// <summary>
/// Subtract two vectors
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The result of adding the two vectors</returns>
public static Vector2 operator -(Vector2 v1, Vector2 v2)
{
Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y);
return v;
}
/// <summary>
/// Negate the vector
/// </summary>
/// <param name="v1">The vector to negate</param>
/// <returns>The negated vector</returns>
/// This will result in a vector pointing in the opposite direction
public static Vector2 operator -(Vector2 v1)
{
Vector2 v = new Vector2(-v1.x, -v1.y);
return v;
}
/// <summary>
/// Scale a vector uniformly up
/// </summary>
/// <param name="v1">The vector to scale</param>
/// <param name="f">The scaling factor</param>
/// <returns>The scaled vector</returns>
/// Each component of the vector will be multipled with the same factor.
public static Vector2 operator *(Vector2 v1, float f)
{
Vector2 v = new Vector2(v1.x * f, v1.y * f);
return v;
}
/// <summary>
/// Scale a vector uniformly up
/// </summary>
/// <param name="f">The scaling factor</param>
/// <param name="v1">The vector to scale</param>
/// <returns>The scaled vector</returns>
/// Each component of the vector will be multipled with the same factor.
public static Vector2 operator *(float f, Vector2 v1)
{
Vector2 v = new Vector2(f * v1.x, f * v1.y);
return v;
}
/// <summary>
/// Scale a vector uniformly down
/// </summary>
/// <param name="v1">The vector to scale</param>
/// <param name="f">The scaling factor</param>
/// <returns>The scaled vector</returns>
/// Each component of the vector will be devided by the same factor.
public static Vector2 operator /(Vector2 v1, float f)
{
Vector2 v = new Vector2(v1.x / f, v1.y / f);
return v;
}
/// <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(Vector2 v1) => x == v1.x && y == v1.y;
/// <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 Vector2 v))
return false;
return (x == v.x && y == v.y);
}
/// <summary>
/// Tests if the two vectors have equal values
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns><em>true</em>when the vectors have equal values</returns>
/// Note that this uses a Float equality check which cannot be not exact in all cases.
/// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
/// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
public static bool operator ==(Vector2 v1, Vector2 v2)
{
return (v1.x == v2.x && v1.y == v2.y);
}
/// <summary>
/// Tests if two vectors have different values
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns><em>true</em>when the vectors have different values</returns>
/// Note that this uses a Float equality check which cannot be not exact in all case.
/// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
/// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
public static bool operator !=(Vector2 v1, Vector2 v2)
{
return (v1.x != v2.x || v1.y != v2.y);
}
/// <summary>
/// Get an hash code for the vector
/// </summary>
/// <returns>The hash code</returns>
public override int GetHashCode()
{
return (x, y).GetHashCode();
}
/// <summary>
/// Get the distance between two vectors
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The distance between the two vectors</returns>
public static float Distance(Vector2 v1, Vector2 v2)
{
float x = v1.x - v2.x;
float y = v1.y - v2.y;
float d = (float)Math.Sqrt(x * x + y * y);
return d;
}
/// <summary>
/// The dot product of two vectors
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The dot product of the two vectors</returns>
public static float Dot(Vector2 v1, Vector2 v2)
{
return v1.x * v2.x + v1.y * v2.y;
}
/// <summary>
/// Lerp between two vectors
/// </summary>
/// <param name="v1">The from vector</param>
/// <param name="v2">The to vector</param>
/// <param name="f">The interpolation distance [0..1]</param>
/// <returns>The lerped vector</returns>
/// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1
/// matches the *v2* vector Value -1 is *v1* vector minus the difference
/// between *v1* and *v2* etc.
public static Vector2 Lerp(Vector2 v1, Vector2 v2, float f)
{
Vector2 v = v1 + (v2 - v1) * f;
return v;
}
/// <summary>
/// Calculate the signed angle between two vectors.
/// </summary>
/// <param name="from">The starting vector</param>
/// <param name="to">The ending vector</param>
/// <param name="axis">The axis to rotate around</param>
/// <returns>The signed angle in degrees</returns>
public static float SignedAngle(Vector2 from, Vector2 to)
{
//float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y);
//return Vector2.Angle(v1, v2) * sign;
float sqrMagFrom = from.sqrMagnitude;
float sqrMagTo = to.sqrMagnitude;
if (sqrMagFrom == 0 || sqrMagTo == 0)
return 0;
//if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
// return nanf("");
float angleFrom = (float)Math.Atan2(from.y, from.x);
float angleTo = (float)Math.Atan2(to.y, to.x);
return (angleTo - angleFrom) * Angle.Rad2Deg;
}
/// <summary>
/// Rotates the vector with the given angle
/// </summary>
/// <param name="v1">The vector to rotate</param>
/// <param name="angle">The angle in degrees</param>
/// <returns></returns>
public static Vector2 Rotate(Vector2 v1, float angle)
{
float sin = (float)Math.Sin(angle * Angle.Deg2Rad);
float cos = (float)Math.Cos(angle * Angle.Deg2Rad);
float tx = v1.x;
float ty = v1.y;
Vector2 v = new Vector2()
{
x = (cos * tx) - (sin * ty),
y = (sin * tx) + (cos * ty)
};
return v;
}
/// <summary>
/// Map interval of angles between vectors [0..Pi] to interval [0..1]
/// </summary>
/// <param name="v1">The first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>The resulting factor in interval [0..1]</returns>
/// Vectors a and b must be normalized
public static float ToFactor(Vector2 v1, Vector2 v2)
{
return (1 - Vector2.Dot(v1, v2)) / 2;
}
}
}

204
src/Vector3.cs Normal file
View File

@ -0,0 +1,204 @@
#if !UNITY_5_3_OR_NEWER
using System;
namespace LinearAlgebra
{
public class Vector3Of<T>
{
public T x;
public T y;
public T z;
public Vector3Of(T x, T y, T z)
{
this.x = x;
this.y = y;
this.z = z;
}
// public uint magnitude {
// get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
// }
}
public class Vector3Int : Vector3Of<int>
{
public Vector3Int(int x, int y, int z) : base(x, y, z) { }
}
public class Vector3Float : Vector3Of<float>
{
public Vector3Float(float x, float y, float z) : base(x, y, z) { }
public float magnitude
{
get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
}
}
/// <summary>
/// 3-dimensional vectors
/// </summary>
/// This uses the right-handed coordinate system.
public struct Vector3 : IEquatable<Vector3>
{
/// <summary>
/// The right axis of the vector
/// </summary>
public float x; //> left/right
/// <summary>
/// The upward axis of the vector
/// </summary>
public float y; //> up/down
/// <summary>
/// The forward axis of the vector
/// </summary>
public float z; //> forward/backward
/// <summary>
/// Create a new 3-dimensional vector
/// </summary>
/// <param name="x">x axis value</param>
/// <param name="y">y axis value</param>
/// <param name="z">z axis value</param>
public Vector3(float x, float y, float z)
{
this.x = x;
this.y = y;
this.z = z;
}
/// <summary>
/// A vector with zero for all axis
/// </summary>
public static readonly Vector3 zero = new Vector3(0, 0, 0);
/// <summary>
/// A vector with one for all axis
/// </summary>
public static readonly Vector3 one = new Vector3(1, 1, 1);
/// <summary>
/// A vector3 with values (-1, 0, 0)
/// </summary>
public static readonly Vector3 left = new Vector3(-1, 0, 0);
/// <summary>
/// A vector with values (1, 0, 0)
/// </summary>
public static readonly Vector3 right = new Vector3(1, 0, 0);
/// <summary>
/// A vector with values (0, -1, 0)
/// </summary>
public static readonly Vector3 down = new Vector3(0, -1, 0);
/// <summary>
/// A vector with values (0, 1, 0)
/// </summary>
public static readonly Vector3 up = new Vector3(0, 1, 0);
/// <summary>
/// A vector with values (0, 0, -1)
/// </summary>
public static readonly Vector3 back = new Vector3(0, -1, 0);
/// <summary>
/// A vector with values (0, 0, 1)
/// </summary>
public static readonly Vector3 forward = new Vector3(0, 1, 0);
public float magnitude
{
get
{
float d = (float)Math.Sqrt(x * x + y * y);
return d;
}
}
public Vector3 normalized
{
get
{
float l = magnitude;
Vector3 v = zero;
if (l > Float.epsilon)
v = this / l;
return v;
}
}
public static Vector3 operator +(Vector3 v1, Vector3 v2)
{
Vector3 v = new Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
return v;
}
public static Vector3 operator -(Vector3 v1, Vector3 v2)
{
Vector3 v = new Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
return v;
}
public static Vector3 operator -(Vector3 v1)
{
Vector3 v = new Vector3(-v1.x, -v1.y, -v1.z);
return v;
}
public static Vector3 operator *(Vector3 v1, float d)
{
Vector3 v = new Vector3(v1.x * d, v1.y * d, v1.z * d);
return v;
}
public static Vector3 operator *(float d, Vector3 v1)
{
Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z);
return v;
}
public static Vector3 operator /(Vector3 v1, float d)
{
Vector3 v = new Vector3(v1.x / d, v1.y / d, v1.z / d);
return v;
}
public bool Equals(Vector3 v) => (x == v.x && y == v.y && z == v.z);
public override bool Equals(object obj)
{
if (!(obj is Vector3 v))
return false;
return (x == v.x && y == v.y && z == v.z);
}
public static bool operator ==(Vector3 v1, Vector3 v2)
{
return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z);
}
public static bool operator !=(Vector3 v1, Vector3 v2)
{
return (v1.x != v2.x || v1.y != v2.y || v1.z != v2.z);
}
public override int GetHashCode()
{
return (x, y, z).GetHashCode();
}
public static float Distance(Vector3 v1, Vector3 v2)
{
return (v2 - v1).magnitude;
}
public static float Dot(Vector3 v1, Vector3 v2)
{
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
public static Vector3 Lerp(Vector3 v1, Vector3 v2, float f)
{
Vector3 v = v1 + (v2 - v1) * f;
return v;
}
}
}
#endif

344
src/float16.cs Normal file
View File

@ -0,0 +1,344 @@
using System;
namespace LinearAlgebra
{
public class float16
{
//
// FILE: float16.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.1.8
// PURPOSE: library for Float16s for Arduino
// URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format
ushort _value;
public float16() { _value = 0; }
public float16(float f)
{
//_value = f32tof16(f);
_value = F32ToF16__(f);
}
public float toFloat()
{
return f16tof32(_value);
}
public ushort GetBinary() { return _value; }
public void SetBinary(ushort value) { _value = value; }
//////////////////////////////////////////////////////////
//
// EQUALITIES
//
/*
bool float16::operator ==(const float16 &f) { return (_value == f._value); }
bool float16::operator !=(const float16 &f) { return (_value != f._value); }
bool float16::operator >(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value < f._value;
if (_value & 0x8000)
return false;
if (f._value & 0x8000)
return true;
return _value > f._value;
}
bool float16::operator >=(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value <= f._value;
if (_value & 0x8000)
return false;
if (f._value & 0x8000)
return true;
return _value >= f._value;
}
bool float16::operator <(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value > f._value;
if (_value & 0x8000)
return true;
if (f._value & 0x8000)
return false;
return _value < f._value;
}
bool float16::operator <=(const float16 &f) {
if ((_value & 0x8000) && (f._value & 0x8000))
return _value >= f._value;
if (_value & 0x8000)
return true;
if (f._value & 0x8000)
return false;
return _value <= f._value;
}
//////////////////////////////////////////////////////////
//
// NEGATION
//
float16 float16::operator -() {
float16 f16;
f16.setBinary(_value ^ 0x8000);
return f16;
}
//////////////////////////////////////////////////////////
//
// MATH
//
float16 float16::operator +(const float16 &f) {
return float16(this->toDouble() + f.toDouble());
}
float16 float16::operator -(const float16 &f) {
return float16(this->toDouble() - f.toDouble());
}
float16 float16::operator *(const float16 &f) {
return float16(this->toDouble() * f.toDouble());
}
float16 float16::operator /(const float16 &f) {
return float16(this->toDouble() / f.toDouble());
}
float16 & float16::operator+=(const float16 &f) {
*this = this->toDouble() + f.toDouble();
return *this;
}
float16 & float16::operator-=(const float16 &f) {
*this = this->toDouble() - f.toDouble();
return *this;
}
float16 & float16::operator*=(const float16 &f) {
*this = this->toDouble() * f.toDouble();
return *this;
}
float16 & float16::operator/=(const float16 &f) {
*this = this->toDouble() / f.toDouble();
return *this;
}
//////////////////////////////////////////////////////////
//
// MATH HELPER FUNCTIONS
//
int float16::sign() {
if (_value & 0x8000)
return -1;
if (_value & 0xFFFF)
return 1;
return 0;
}
bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); }
bool float16::isNaN() {
if ((_value & 0x7C00) != 0x7C00)
return false;
if ((_value & 0x03FF) == 0x0000)
return false;
return true;
}
bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); }
*/
//////////////////////////////////////////////////////////
//
// CORE CONVERSION
//
float f16tof32(ushort _value)
{
//ushort sgn;
ushort man;
int exp;
float f;
//Debug.Log($"{_value}");
bool sgn = (_value & 0x8000) > 0;
exp = (_value & 0x7C00) >> 10;
man = (ushort)(_value & 0x03FF);
//Debug.Log($"{sgn} {exp} {man}");
// ZERO
if ((_value & 0x7FFF) == 0)
{
return sgn ? -0 : 0;
}
// NAN & INF
if (exp == 0x001F)
{
if (man == 0)
return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY;
else
return float.NaN; // NAN;
}
// SUBNORMAL/NORMAL
if (exp == 0)
f = 0;
else
f = 1;
// PROCESS MANTISSE
for (int i = 9; i >= 0; i--)
{
f *= 2;
if ((man & (1 << i)) != 0)
f = f + 1;
}
//Debug.Log($"{f}");
f = f * (float)Math.Pow(2.0f, exp - 25);
if (exp == 0)
{
f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8;
}
//Debug.Log($"{f}");
return sgn ? -f : f;
}
public static uint SingleToInt32Bits(float value)
{
byte[] bytes = BitConverter.GetBytes(value);
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes); // If the system is little-endian, reverse the byte order
return BitConverter.ToUInt32(bytes, 0);
}
public ushort F32ToF16__(float f)
{
uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0);
ushort man = (ushort)((t & 0x007FFFFF) >> 12);
int exp = (int)((t & 0x7F800000) >> 23);
bool sgn = (t & 0x80000000) != 0;
// handle 0
if ((t & 0x7FFFFFFF) == 0)
{
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// denormalized float32 does not fit in float16
if (exp == 0x00)
{
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// handle infinity & NAN
if (exp == 0x00FF)
{
if (man != 0)
return 0xFE00; // NAN
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// normal numbers
exp = exp - 127 + 15;
// overflow does not fit => INF
if (exp > 30)
{
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// subnormal numbers
if (exp < -38)
{
return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal
{
man >>= (exp + 14);
// rounding
man++;
man >>= 1;
if (sgn)
return (ushort)(0x8000 | man);
return man;
}
// normal
// TODO rounding
exp <<= 10;
man++;
man >>= 1;
if (sgn)
return (ushort)(0x8000 | exp | man);
return (ushort)(exp | man);
}
//This function is faulty!!!!
ushort f32tof16(float f)
{
//uint t = *(uint*)&f;
//uint t = (uint)BitConverter.SingleToInt32Bits(f);
uint t = SingleToInt32Bits(f);
// man bits = 10; but we keep 11 for rounding
ushort man = (ushort)((t & 0x007FFFFF) >> 12);
short exp = (short)((t & 0x7F800000) >> 23);
bool sgn = (t & 0x80000000) != 0;
// handle 0
if ((t & 0x7FFFFFFF) == 0)
{
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// denormalized float32 does not fit in float16
if (exp == 0x00)
{
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// handle infinity & NAN
if (exp == 0x00FF)
{
if (man != 0)
return 0xFE00; // NAN
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// normal numbers
exp = (short)(exp - 127 + 15);
// overflow does not fit => INF
if (exp > 30)
{
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// subnormal numbers
if (exp < -38)
{
return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal
{
man >>= (exp + 14);
// rounding
man++;
man >>= 1;
if (sgn)
return (ushort)(0x8000 | man);
return man;
}
// normal
// TODO rounding
exp <<= 10;
man++;
man >>= 1;
ushort uexp = (ushort)exp;
if (sgn)
return (ushort)(0x8000 | uexp | man);
return (ushort)(uexp | man);
}
// -- END OF FILE --
}
}

171
test/AngleTest.cs Normal file
View File

@ -0,0 +1,171 @@
#if !UNITY_5_6_OR_NEWER
using NUnit.Framework;
namespace LinearAlgebra.Test
{
public class Tests
{
[SetUp]
public void Setup()
{
}
[Test]
public void Test_Normalize()
{
float r = 0;
r = Angle.Normalize(90);
Assert.AreEqual(r, 90, "Normalize 90");
r = Angle.Normalize(-90);
Assert.AreEqual(r, -90, "Normalize -90");
r = Angle.Normalize(270);
Assert.AreEqual(r, -90, "Normalize 270");
r = Angle.Normalize(270 + 360);
Assert.AreEqual(r, -90, "Normalize 270+360");
r = Angle.Normalize(-270);
Assert.AreEqual(r, 90, "Normalize -270");
r = Angle.Normalize(-270 - 360);
Assert.AreEqual(r, 90, "Normalize -270-360");
r = Angle.Normalize(0);
Assert.AreEqual(r, 0, "Normalize 0");
r = Angle.Normalize(float.PositiveInfinity);
Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY");
r = Angle.Normalize(float.NegativeInfinity);
Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY");
}
[Test]
public void Clamp()
{
float r = 0;
r = Angle.Clamp(1, 0, 2);
Assert.AreEqual(r, 1, "Clamp 1 0 2");
r = Angle.Clamp(-1, 0, 2);
Assert.AreEqual(r, 0, "Clamp -1 0 2");
r = Angle.Clamp(3, 0, 2);
Assert.AreEqual(r, 2, "Clamp 3 0 2");
r = Angle.Clamp(1, 0, 0);
Assert.AreEqual(r, 0, "Clamp 1 0 0");
r = Angle.Clamp(0, 0, 0);
Assert.AreEqual(r, 0, "Clamp 0 0 0");
r = Angle.Clamp(0, 1, -1);
Assert.AreEqual(r, 1, "Clamp 0 1 -1");
r = Angle.Clamp(1, 0, float.PositiveInfinity);
Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY");
r = Angle.Clamp(1, float.NegativeInfinity, 1);
Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1");
}
[Test]
public void Difference()
{
float r = 0;
r = Angle.Difference(0, 90);
Assert.AreEqual(r, 90, "Difference 0 90");
r = Angle.Difference(0, -90);
Assert.AreEqual(r, -90, "Difference 0 -90");
r = Angle.Difference(0, 270);
Assert.AreEqual(r, -90, "Difference 0 270");
r = Angle.Difference(0, -270);
Assert.AreEqual(r, 90, "Difference 0 -270");
r = Angle.Difference(90, 0);
Assert.AreEqual(r, -90, "Difference 90 0");
r = Angle.Difference(-90, 0);
Assert.AreEqual(r, 90, "Difference -90 0");
r = Angle.Difference(0, 0);
Assert.AreEqual(r, 0, "Difference 0 0");
r = Angle.Difference(90, 90);
Assert.AreEqual(r, 0, "Difference 90 90");
r = Angle.Difference(0, float.PositiveInfinity);
Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY");
r = Angle.Difference(0, float.NegativeInfinity);
Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY");
r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity);
Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY");
}
[Test]
public void MoveTowards()
{
float r = 0;
r = Angle.MoveTowards(0, 90, 30);
Assert.AreEqual(r, 30, "MoveTowards 0 90 30");
r = Angle.MoveTowards(0, 90, 90);
Assert.AreEqual(r, 90, "MoveTowards 0 90 90");
r = Angle.MoveTowards(0, 90, 180);
Assert.AreEqual(r, 90, "MoveTowards 0 90 180");
r = Angle.MoveTowards(0, 90, 270);
Assert.AreEqual(r, 90, "MoveTowrads 0 90 270");
r = Angle.MoveTowards(0, 90, -30);
Assert.AreEqual(r, -30, "MoveTowards 0 90 -30");
r = Angle.MoveTowards(0, -90, -30);
Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30");
r = Angle.MoveTowards(0, -90, -90);
Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90");
r = Angle.MoveTowards(0, -90, -180);
Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180");
r = Angle.MoveTowards(0, -90, -270);
Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270");
r = Angle.MoveTowards(0, 90, 0);
Assert.AreEqual(r, 0, "MoveTowards 0 90 0");
r = Angle.MoveTowards(0, 0, 0);
Assert.AreEqual(r, 0, "MoveTowards 0 0 0");
r = Angle.MoveTowards(0, 0, 30);
Assert.AreEqual(r, 0, "MoveTowrads 0 0 30");
r = Angle.MoveTowards(0, 90, float.PositiveInfinity);
Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY");
r = Angle.MoveTowards(0, float.PositiveInfinity, 30);
Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30");
r = Angle.MoveTowards(0, -90, float.NegativeInfinity);
Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY");
r = Angle.MoveTowards(0, float.NegativeInfinity, -30);
Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30");
}
}
}
#endif

19
test/LinearAlgebra_Test.csproj Normal file
View File

@ -0,0 +1,19 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net5.0</TargetFramework>
<IsPackable>false</IsPackable>
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
<PackageReference Include="NUnit" Version="3.13.2" />
<PackageReference Include="NUnit3TestAdapter" Version="3.17.0" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\src\LinearAlgebra.csproj" />
</ItemGroup>
</Project>