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

Added RoboidControl repo

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-24 14:13:54 +02:00
commit 7d70a83ec2
59 changed files with 8621 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
.editorconfig Normal file
View File

@ -0,0 +1,19 @@
# EditorConfig is awesome: https://EditorConfig.org
# top-most EditorConfig file
root = true
[*]
indent_style = space
indent_size = 4
end_of_line = crlf
charset = utf-8
trim_trailing_whitespace = false
insert_final_newline = false
max_line_length = 80
[*.cs]
csharp_new_line_before_open_brace = none
# Suppress warnings everywhere
dotnet_diagnostic.IDE1006.severity = none
dotnet_diagnostic.IDE0130.severity = none

5
.gitignore vendored Normal file
View File

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

26
.gitlab-ci.yml Normal file
View File

@ -0,0 +1,26 @@
# You can override the included template(s) by including variable overrides
# SAST customization: https://docs.gitlab.com/ee/user/application_security/sast/#customizing-the-sast-settings
# Secret Detection customization: https://docs.gitlab.com/ee/user/application_security/secret_detection/pipeline/#customization
# Dependency Scanning customization: https://docs.gitlab.com/ee/user/application_security/dependency_scanning/#customizing-the-dependency-scanning-settings
# Container Scanning customization: https://docs.gitlab.com/ee/user/application_security/container_scanning/#customizing-the-container-scanning-settings
# Note that environment variables can be set in several places
# See https://docs.gitlab.com/ee/ci/variables/#cicd-variable-precedence
stages:
- build
- test
- deploy
- review
- dast
- staging
- canary
- production
- incremental rollout 10%
- incremental rollout 25%
- incremental rollout 50%
- incremental rollout 100%
- performance
- cleanup
sast:
stage: test
include:
- template: Auto-DevOps.gitlab-ci.yml

14
.vscode/launch.json vendored Normal file
View File

@ -0,0 +1,14 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "C#: BB2B Debug",
"type": "dotnet",
"request": "launch",
"projectPath": "${workspaceFolder}/Examples/BB2B/BB2B.csproj"
}
]
}

2861
DoxyGen/Doxyfile Normal file
View File

File diff suppressed because it is too large Load Diff

226
DoxyGen/DoxygenLayout.xml Normal file
View File

@ -0,0 +1,226 @@
<doxygenlayout version="1.0">
<!-- Generated by doxygen 1.8.18 -->
<!-- Navigation index tabs for HTML output -->
<navindex>
<tab type="mainpage" visible="yes" title=""/>
<tab type="pages" visible="yes" title="" intro=""/>
<tab type="modules" visible="yes" title="" intro=""/>
<tab type="namespaces" visible="yes" title="">
<tab type="namespacelist" visible="yes" title="" intro=""/>
<tab type="namespacemembers" visible="yes" title="" intro=""/>
</tab>
<tab type="interfaces" visible="yes" title="">
<tab type="interfacelist" visible="yes" title="" intro=""/>
<tab type="interfaceindex" visible="$ALPHABETICAL_INDEX" title=""/>
<tab type="interfacehierarchy" visible="yes" title="" intro=""/>
</tab>
<tab type="classes" visible="yes" title="">
<tab type="classlist" visible="yes" title="" intro=""/>
<tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/>
<tab type="hierarchy" visible="yes" title="" intro=""/>
<tab type="classmembers" visible="yes" title="" intro=""/>
</tab>
<tab type="structs" visible="yes" title="">
<tab type="structlist" visible="yes" title="" intro=""/>
<tab type="structindex" visible="$ALPHABETICAL_INDEX" title=""/>
</tab>
<tab type="exceptions" visible="yes" title="">
<tab type="exceptionlist" visible="yes" title="" intro=""/>
<tab type="exceptionindex" visible="$ALPHABETICAL_INDEX" title=""/>
<tab type="exceptionhierarchy" visible="yes" title="" intro=""/>
</tab>
<tab type="files" visible="yes" title="">
<tab type="filelist" visible="yes" title="" intro=""/>
<tab type="globals" visible="yes" title="" intro=""/>
</tab>
<tab type="examples" visible="yes" title="" intro=""/>
</navindex>
<!-- Layout definition for a class page -->
<class>
<briefdescription visible="no"/>
<detaileddescription title=""/>
<includes visible="$SHOW_INCLUDE_FILES"/>
<inheritancegraph visible="$CLASS_GRAPH"/>
<collaborationgraph visible="$COLLABORATION_GRAPH"/>
<memberdecl>
<nestedclasses visible="yes" title=""/>
<publictypes title=""/>
<services title=""/>
<interfaces title=""/>
<publicslots title=""/>
<signals title=""/>
<publicmethods title=""/>
<publicstaticmethods title=""/>
<publicattributes title=""/>
<publicstaticattributes title=""/>
<protectedtypes title=""/>
<protectedslots title=""/>
<protectedmethods title=""/>
<protectedstaticmethods title=""/>
<protectedattributes title=""/>
<protectedstaticattributes title=""/>
<packagetypes title=""/>
<packagemethods title=""/>
<packagestaticmethods title=""/>
<packageattributes title=""/>
<packagestaticattributes title=""/>
<properties title=""/>
<events title=""/>
<privatetypes title=""/>
<privateslots title=""/>
<privatemethods title=""/>
<privatestaticmethods title=""/>
<privateattributes title=""/>
<privatestaticattributes title=""/>
<friends title=""/>
<related title="" subtitle=""/>
<membergroups visible="yes"/>
</memberdecl>
<memberdef>
<inlineclasses title=""/>
<typedefs title=""/>
<enums title=""/>
<services title=""/>
<interfaces title=""/>
<constructors title=""/>
<functions title=""/>
<related title=""/>
<variables title=""/>
<properties title=""/>
<events title=""/>
</memberdef>
<allmemberslink visible="yes"/>
<usedfiles visible="$SHOW_USED_FILES"/>
<authorsection visible="yes"/>
</class>
<!-- Layout definition for a namespace page -->
<namespace>
<briefdescription visible="yes"/>
<memberdecl>
<nestednamespaces visible="yes" title=""/>
<constantgroups visible="yes" title=""/>
<interfaces visible="yes" title=""/>
<classes visible="yes" title=""/>
<structs visible="yes" title=""/>
<exceptions visible="yes" title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
<membergroups visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
<memberdef>
<inlineclasses title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
</memberdef>
<authorsection visible="yes"/>
</namespace>
<!-- Layout definition for a file page -->
<file>
<briefdescription visible="yes"/>
<includes visible="$SHOW_INCLUDE_FILES"/>
<includegraph visible="$INCLUDE_GRAPH"/>
<includedbygraph visible="$INCLUDED_BY_GRAPH"/>
<sourcelink visible="yes"/>
<memberdecl>
<interfaces visible="yes" title=""/>
<classes visible="yes" title=""/>
<structs visible="yes" title=""/>
<exceptions visible="yes" title=""/>
<namespaces visible="yes" title=""/>
<constantgroups visible="yes" title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
<membergroups visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
<memberdef>
<inlineclasses title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<functions title=""/>
<variables title=""/>
</memberdef>
<authorsection/>
</file>
<!-- Layout definition for a group page -->
<group>
<briefdescription visible="yes"/>
<groupgraph visible="$GROUP_GRAPHS"/>
<memberdecl>
<nestedgroups visible="yes" title=""/>
<dirs visible="yes" title=""/>
<files visible="yes" title=""/>
<namespaces visible="yes" title=""/>
<classes visible="yes" title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<enumvalues title=""/>
<functions title=""/>
<variables title=""/>
<signals title=""/>
<publicslots title=""/>
<protectedslots title=""/>
<privateslots title=""/>
<events title=""/>
<properties title=""/>
<friends title=""/>
<membergroups visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
<memberdef>
<pagedocs/>
<inlineclasses title=""/>
<defines title=""/>
<typedefs title=""/>
<sequences title=""/>
<dictionaries title=""/>
<enums title=""/>
<enumvalues title=""/>
<functions title=""/>
<variables title=""/>
<signals title=""/>
<publicslots title=""/>
<protectedslots title=""/>
<privateslots title=""/>
<events title=""/>
<properties title=""/>
<friends title=""/>
</memberdef>
<authorsection visible="yes"/>
</group>
<!-- Layout definition for a directory page -->
<directory>
<briefdescription visible="yes"/>
<directorygraph visible="yes"/>
<memberdecl>
<dirs visible="yes"/>
<files visible="yes"/>
</memberdecl>
<detaileddescription title=""/>
</directory>
</doxygenlayout>

12
DoxyGen/custom_doxygen.css Normal file
View File

@ -0,0 +1,12 @@
/* Custom PasserVR CSS for DoxyGen */
a {
color: #e77505;
}
.contents a:visited {
color: #e77505;
}
a:hover {
color: #10659C;
}

31
Examples/BB2B/BB2B.cs Normal file
View File

@ -0,0 +1,31 @@
using System.Threading;
using RoboidControl;
class BB2B {
static void Main() {
// The robot's propulsion is a differential drive
DifferentialDrive bb2b = new();
// Is has a touch sensor at the front left of the roboid
TouchSensor touchLeft = new(bb2b);
// and other one on the right
TouchSensor touchRight = new(bb2b);
// Do forever:
while (true) {
// The left wheel turns forward when nothing is touched on the right side
// and turn backward when the roboid hits something on the right
float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
// Update the roboid state
bb2b.Update(true);
// and sleep for 100ms
Thread.Sleep(100);
}
}
}

11
Examples/BB2B/BB2B.csproj Normal file
View File

@ -0,0 +1,11 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net5.0</TargetFramework>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\..\src\RoboidControl.csproj" />
</ItemGroup>
</Project>

19
LinearAlgebra/.editorconfig Normal file
View File

@ -0,0 +1,19 @@
# EditorConfig is awesome: https://EditorConfig.org
# top-most EditorConfig file
root = true
[*]
indent_style = space
indent_size = 4
end_of_line = crlf
charset = utf-8
trim_trailing_whitespace = false
insert_final_newline = false
max_line_length = 80
[*.cs]
csharp_new_line_before_open_brace = none
# Suppress warnings everywhere
dotnet_diagnostic.IDE1006.severity = none
dotnet_diagnostic.IDE0130.severity = none

6
LinearAlgebra/.gitignore vendored Normal file
View File

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

110
LinearAlgebra/src/Angle.cs Normal file
View File

@ -0,0 +1,110 @@
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);
//}
}
}

287
LinearAlgebra/src/Decomposition.cs Normal file
View File

@ -0,0 +1,287 @@
using System;
namespace LinearAlgebra {
class QR {
// QR Decomposition of a matrix A
public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) {
int nRows = A.nRows;
int nCols = A.nCols;
float[,] Q = new float[nRows, nCols];
float[,] R = new float[nCols, nCols];
// Perform Gram-Schmidt orthogonalization
for (uint colIx = 0; colIx < nCols; colIx++) {
// Step 1: v = column(ix) of A
float[] v = new float[nRows];
for (int rowIx = 0; rowIx < nRows; rowIx++)
v[rowIx] = A.data[rowIx, colIx];
// Step 2: Subtract projections of v onto previous q's (orthogonalize)
for (uint colIx2 = 0; colIx2 < colIx; colIx2++) {
float dotProd = 0;
for (int i = 0; i < nRows; i++)
dotProd += Q[i, colIx2] * v[i];
for (int i = 0; i < nRows; i++)
v[i] -= dotProd * Q[i, colIx2];
}
// Step 3: Normalize v to get column(ix) of Q
float norm = 0;
for (int rowIx = 0; rowIx < nRows; rowIx++)
norm += v[rowIx] * v[rowIx];
norm = (float)Math.Sqrt(norm);
for (int rowIx = 0; rowIx < nRows; rowIx++)
Q[rowIx, colIx] = v[rowIx] / norm;
// Store the coefficients of R
for (int colIx2 = 0; colIx2 <= colIx; colIx2++) {
R[colIx2, colIx] = 0;
for (int k = 0; k < nRows; k++)
R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx];
}
}
return (new Matrix2(Q), new Matrix2(R));
}
// Reduced QR Decomposition of a matrix A
public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) {
int nRows = A.nRows;
int nCols = A.nCols;
float[,] Q = new float[nRows, nCols];
float[,] R = new float[nCols, nCols];
// Perform Gram-Schmidt orthogonalization
for (int colIx = 0; colIx < nCols; colIx++) {
// Step 1: v = column(colIx) of A
float[] columnIx = new float[nRows];
bool isZeroColumn = true;
for (int rowIx = 0; rowIx < nRows; rowIx++) {
columnIx[rowIx] = A.data[rowIx, colIx];
if (columnIx[rowIx] != 0)
isZeroColumn = false;
}
if (isZeroColumn) {
for (int rowIx = 0; rowIx < nRows; rowIx++)
Q[rowIx, colIx] = 0;
// Set corresponding R element to 0
R[colIx, colIx] = 0;
Console.WriteLine($"zero column {colIx}");
continue;
}
// Step 2: Subtract projections of v onto previous q's (orthogonalize)
for (int colIx2 = 0; colIx2 < colIx; colIx2++) {
// Compute the dot product of v and column(colIx2) of Q
float dotProduct = 0;
for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2];
// Subtract the projection from v
for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2];
}
// Step 3: Normalize v to get column(colIx) of Q
float norm = 0;
for (int rowIx = 0; rowIx < nRows; rowIx++)
norm += columnIx[rowIx] * columnIx[rowIx];
if (norm == 0)
throw new Exception("invalid value");
norm = (float)Math.Sqrt(norm);
for (int rowIx = 0; rowIx < nRows; rowIx++)
Q[rowIx, colIx] = columnIx[rowIx] / norm;
// Here is where it deviates from the Full QR Decomposition !
// Step 4: Compute the row(colIx) of R
for (int colIx2 = colIx; colIx2 < nCols; colIx2++) {
float dotProduct = 0;
for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2];
R[colIx, colIx2] = dotProduct;
}
}
if (!float.IsFinite(R[0, 0]))
throw new Exception("invalid value");
return (new Matrix2(Q), new Matrix2(R));
}
}
class SVD {
// According to ChatGPT, Mathnet uses Golub-Reinsch SVD algorithm
// 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization.
// This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix.
// This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD.
//
// 2. Diagonalization: Once the matrix is in bidiagonal form,
// the singular values are computed using an iterative process
// (typically involving QR factorization or Jacobi rotations) until convergence.
// This process diagonalizes the bidiagonal matrix and allows extraction of the singular values.
//
// 3. Computing UU and VTVT: After obtaining the singular values,
// the left singular vectors UU and right singular vectors VTVT are computed
// using the accumulated transformations (such as Householder reflections) from the bidiagonalization step.
// Bidiagnolizations through Householder transformations
public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) {
int m = A.nRows; // Rows of A
int n = A.nCols; // Columns of A
float[,] U1 = new float[m, m]; // Left orthogonal matrix
float[,] V1 = new float[n, n]; // Right orthogonal matrix
float[,] B = A.Clone().data; // Copy A to B for transformation
// Initialize U1 and V1 as identity matrices
for (int i = 0; i < m; i++)
U1[i, i] = 1;
for (int i = 0; i < n; i++)
V1[i, i] = 1;
// Perform Householder reflections to create a bidiagonal matrix B
for (int j = 0; j < n; j++) {
// Step 1: Construct the Householder vector y
float[] y = new float[m - j];
for (int i = j; i < m; i++)
y[i - j] = B[i, j];
// Step 2: Compute the norm and scalar alpha
float norm = 0;
for (int i = 0; i < y.Length; i++)
norm += y[i] * y[i];
norm = (float)Math.Sqrt(norm);
if (B[j, j] > 0)
norm = -norm;
float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j])));
float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
// Step 3: Apply the reflection to zero out below diagonal
for (int k = j; k < n; k++) {
float dot = 0;
for (int i = j; i < m; i++)
dot += y[i - j] * B[i, k];
dot /= r;
for (int i = j; i < m; i++)
B[i, k] -= 2 * dot * y[i - j];
}
// Step 4: Update U1 with the Householder reflection (U1 * Householder)
for (int i = j; i < m; i++)
U1[i, j] = y[i - j] / alpha;
// Step 5: Update V1 (storing the Householder vector y)
// Correct indexing: we only need to store part of y in V1 from index j to n
for (int i = j; i < n; i++)
V1[j, i] = B[j, i];
// Repeat steps for further columns if necessary
}
return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1));
}
public static Matrix2 Bidiagonalize(Matrix2 A) {
int m = A.nRows; // Rows of A
int n = A.nCols; // Columns of A
float[,] B = A.Clone().data; // Copy A to B for transformation
// Perform Householder reflections to create a bidiagonal matrix B
for (int j = 0; j < n; j++) {
// Step 1: Construct the Householder vector y
float[] y = new float[m - j];
for (int i = j; i < m; i++)
y[i - j] = B[i, j];
// Step 2: Compute the norm and scalar alpha
float norm = 0;
for (int i = 0; i < y.Length; i++)
norm += y[i] * y[i];
norm = (float)Math.Sqrt(norm);
if (B[j, j] > 0)
norm = -norm;
float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
// Step 3: Apply the reflection to zero out below diagonal
for (int k = j; k < n; k++) {
float dot = 0;
for (int i = j; i < m; i++)
dot += y[i - j] * B[i, k];
dot /= r;
for (int i = j; i < m; i++)
B[i, k] -= 2 * dot * y[i - j];
}
// Repeat steps for further columns if necessary
}
return new Matrix2(B);
}
// QR Iteration for diagonalization of a bidiagonal matrix B
public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) {
int m = B.nRows;
int n = B.nCols;
Matrix2 U = new(m, m); // Left singular vectors (U)
Matrix2 Vt = new(n, n); // Right singular vectors (V^T)
float[] singularValues = new float[Math.Min(m, n)]; // Singular values
// Initialize U and Vt as identity matrices
for (int i = 0; i < m; i++)
U.data[i, i] = 1;
for (int i = 0; i < n; i++)
Vt.data[i, i] = 1;
// Perform QR iterations
float tolerance = 1e-7f; //1e-12f; for double
bool converged = false;
while (!converged) {
// Perform QR decomposition on the matrix B
(Matrix2 Q, Matrix2 R) = QR.Decomposition(B);
// Update B to be the product Q * R //R * Q
B = R * Q;
// Accumulate the transformations in U and Vt
U *= Q;
Vt *= R;
// Check convergence by looking at the off-diagonal elements of B
converged = true;
for (int i = 0; i < m - 1; i++) {
for (int j = i + 1; j < n; j++) {
if (Math.Abs(B.data[i, j]) > tolerance) {
converged = false;
break;
}
}
}
}
// Extract singular values (diagonal elements of B)
for (int i = 0; i < Math.Min(m, n); i++)
singularValues[i] = B.data[i, i];
return (new Matrix1(singularValues), U, Vt);
}
public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) {
if (A.nRows != A.nCols)
throw new ArgumentException("SVD: matrix A has to be square.");
Matrix2 B = Bidiagonalize(A);
(Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B);
return (U, S, Vt);
}
}
}

83
LinearAlgebra/src/Direction.cs Normal file
View File

@ -0,0 +1,83 @@
using System;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
#endif
namespace LinearAlgebra
{
/// <summary>
/// A direction in 3D space
/// </summary>
/// A direction is represented using two angles:
/// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
/// degrees which is a rotation in the horizontal plane
/// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
/// which is the rotation in the up/down direction applied after the horizontal
/// rotation has been applied.
/// The angles are automatically normalized to stay within the abovenmentioned
/// ranges.
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 static Direction Degrees(float horizontal, float vertical) {
Direction d = new() {
horizontal = horizontal,
vertical = vertical
};
//Normalize();
return d;
}
public static Direction Radians(float horizontal, float vertical) {
Direction d = new() {
horizontal = horizontal * Angle.Rad2Deg,
vertical = vertical * Angle.Rad2Deg
};
//Normalize();
return d;
}
public readonly static Direction forward = Degrees(0, 0);
public readonly static Direction backward = Degrees(-180, 0);
public readonly static Direction up = Degrees(0, 90);
public readonly static Direction down = Degrees(0, -90);
public readonly static Direction left = Degrees(-90, 0);
public readonly static Direction right = Degrees(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;
}
}
}

41
LinearAlgebra/src/Float.cs Normal file
View File

@ -0,0 +1,41 @@
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
LinearAlgebra/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>

689
LinearAlgebra/src/Matrix.cs Normal file
View File

@ -0,0 +1,689 @@
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 int start { get; }
public int stop { get; }
public Slice(int start, int stop)
{
this.start = start;
this.stop = stop;
}
}
public class Matrix2 {
public float[,] data { get; }
public int nRows => data.GetLength(0);
public int nCols => data.GetLength(1);
public Matrix2(int nRows, int 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(int nRows, int 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(int size)
{
return Diagonal(1, size);
}
public static Matrix2 Identity(int nRows, int 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, int 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)
{
int n = (int)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)
{
int 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);
}
public Matrix1 GetRow(int rowIx) {
float[] row = new float[this.nCols];
for (int colIx = 0; colIx < this.nCols; colIx++) {
row[colIx] = this.data[rowIx, colIx];
}
return new Matrix1(row);
}
#if UNITY_5_3_OR_NEWER
public Vector3Float GetRow3(int rowIx) {
int 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 void SwapRows(int row1, int row2) {
for (uint ix = 0; ix < this.nCols; ix++) {
float temp = this.data[row1, ix];
this.data[row1, ix] = this.data[row2, ix];
this.data[row2, ix] = temp;
}
}
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 void SetColumn(int colIx, Matrix1 v) {
for (uint ix = 0; ix < v.size; ix++)
this.data[ix, colIx] = v.data[ix];
}
public void SetColumn(int colIx, Vector3Float v) {
this.data[0, colIx] = v.x;
this.data[1, colIx] = v.y;
this.data[2, colIx] = v.z;
}
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 GetRows(Slice slice) {
return GetRows(slice.start, slice.stop);
}
public Matrix2 GetRows(int from, int 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 (int 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 slice)
{
return Slice(slice.start, slice.stop);
}
public Matrix2 Slice(int from, int 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 (int 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((int start, int stop) rowRange, (int start, int stop) colRange)
{
float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
int resultRowIx = 0;
int resultColIx = 0;
for (int i = rowRange.start; i < rowRange.stop; i++)
{
for (int 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) {
UpdateSlice((slice.start, slice.stop), m);
}
public void UpdateSlice((int start, int stop) slice, Matrix2 m) {
// if (slice.start == slice.stop)
// Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!");
int mRowIx = 0;
for (int 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((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m)
{
for (int i = rowRange.start; i < rowRange.stop; i++)
{
for (int 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
int 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 (int i = n - 1; i >= 0; i--)
{
// Eliminate the column above the pivot
for (int 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()
{
int 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)
{
int 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 static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) {
float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols];
int resultRowIx = 0;
for (int i = 0; i < A.nRows; i++) {
if (i >= rowRange.start && i < rowRange.stop)
continue;
for (int j = 0; j < A.nCols; j++)
result[resultRowIx, j] = A.data[i, j];
resultRowIx++;
}
return new Matrix2(result);
}
internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) {
float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)];
for (int i = 0; i < A.nRows; i++) {
int resultColIx = 0;
for (int j = 0; j < A.nCols; j++) {
if (j >= colRange.start && j < colRange.stop)
continue;
result[i, resultColIx++] = A.data[i, j];
}
}
return new Matrix2(result);
}
}
public class Matrix1
{
public float[] data { get; }
public int size => data.GetLength(0);
public Matrix1(int size)
{
this.data = new float[size];
}
public Matrix1(float[] data) {
this.data = data;
}
public static Matrix1 Zero(int 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, 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 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 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) {
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
result[i] = f / A.data[i];
return new Matrix1(result);
}
public Matrix1 Slice(Slice range)
{
return Slice(range.start, range.stop);
}
public Matrix1 Slice(int from, int 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 (int 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 (int ix = slice.start; ix < slice.stop; ix++, vIx++)
this.data[ix] = v.data[vIx];
}
}
}

87
LinearAlgebra/src/Quat32.cs Normal file
View File

@ -0,0 +1,87 @@
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);
}
}
}
}

81
LinearAlgebra/src/Quaternion.cs Normal file
View File

@ -0,0 +1,81 @@
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 Quaternion Reflect(Quaternion q) {
return new(-q.x, -q.y, -q.z, q.w);
}
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) { }
// }
}

134
LinearAlgebra/src/Spherical.cs Normal file
View File

@ -0,0 +1,134 @@
using System;
#if UNITY_5_3_OR_NEWER
//using Vector3Float = UnityEngine.Vector3;
using Vector3 = UnityEngine.Vector3;
#endif
namespace LinearAlgebra {
/// <summary>
/// A spherical vector
/// </summary>
public class Spherical {
/// <summary>
/// Create a default vector with zero distance
/// </summary>
public Spherical() {
this.distance = 0;
this.direction = new Direction();
}
/// <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) {
this.distance = distance;
this.direction = direction;
}
/// <summary>
/// Create spherical vector. All given angles are in degrees
/// </summary>
/// <param name="distance">The distance in meters</param>
/// <param name="horizontal">The horizontal angle in degrees</param>
/// <param name="vertical">The vertical angle in degrees</param>
/// <returns></returns>
public static Spherical Degrees(float distance, float horizontal, float vertical) {
Direction direction = Direction.Degrees(horizontal, vertical);
Spherical s = new(distance, direction);
return s;
}
public static Spherical Radians(float distance, float horizontal, float vertical) {
Direction direction = Direction.Radians(horizontal, vertical);
Spherical s = new(distance, direction);
return s;
}
/// <summary>
/// The distance in meters
/// </summary>
/// @remark The distance should never be negative
public float distance;
/// <summary>
/// The direction of the vector
/// </summary>
public Direction direction;
/// <summary>
/// A spherical vector with zero degree angles and distance
/// </summary>
public readonly static Spherical zero = new(0, Direction.forward);
/// <summary>
/// A normalized forward-oriented vector
/// </summary>
public readonly static Spherical forward = new(1, Direction.forward);
// 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 FromVector3(Vector3 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 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 Vector3 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;
Vector3 v = new(x, y, z);
return v;
}
public static Spherical operator +(Spherical s1, Spherical s2) {
// let's do it the easy way...
Vector3 v1 = s1.ToVector3();
Vector3 v2 = s2.ToVector3();
Vector3 v = v1 + v2;
Spherical r = FromVector3(v);
return r;
}
}
}

41
LinearAlgebra/src/SwingTwist.cs Normal file
View File

@ -0,0 +1,41 @@
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 = Direction.Degrees(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
}
}

363
LinearAlgebra/src/Vector2.cs Normal file
View File

@ -0,0 +1,363 @@
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;
}
}
}

179
LinearAlgebra/src/Vector3.cs Normal file
View File

@ -0,0 +1,179 @@
#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 readonly float magnitude {
get {
float d = (float)Math.Sqrt(x * x + y * y + z * z);
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

322
LinearAlgebra/src/float16.cs Normal file
View File

@ -0,0 +1,322 @@
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
LinearAlgebra/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 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
LinearAlgebra/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>

30
LinearAlgebra/test/SphericalTest.cs Normal file
View File

@ -0,0 +1,30 @@
#if !UNITY_5_6_OR_NEWER
using NUnit.Framework;
namespace LinearAlgebra.Test {
public class SphericalTest {
[SetUp]
public void Setup() {
}
[Test]
public void FromVector3() {
Vector3 v = new(0, 0, 1);
Spherical s = Spherical.FromVector3(v);
Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1");
Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1");
Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1");
}
[Test]
public void Addition() {
Spherical v1 = Spherical.Degrees(1, 45, 0);
Spherical v2 = Spherical.zero;
Spherical r = Spherical.zero;
r = v1 + v2;
Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)");
}
}
}
#endif

10
README.md Normal file
View File

@ -0,0 +1,10 @@
\mainpage Roboid Control for C#
Roboid Control support for C# applications.
Includes support for the Unity game engine.
# Basic components
- RoboidControl::Thing
- RoboidControl::LocalParticipant
- RoboidControl::SiteServer

8
RoboidControl-csharp.code-workspace Normal file
View File

@ -0,0 +1,8 @@
{
"folders": [
{
"path": "."
}
],
"settings": {}
}

55
RoboidControl.sln Normal file
View File

@ -0,0 +1,55 @@

Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17
VisualStudioVersion = 17.0.31903.59
MinimumVisualStudioVersion = 10.0.40219.1
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RoboidControl_Test", "test\RoboidControl_Test.csproj", "{B65BB41E-5A93-46FC-BA68-B865F50D57BD}"
EndProject
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Examples", "Examples", "{D6086F71-404B-4D18-BBE9-45947ED33DB2}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "BB2B", "Examples\BB2B\BB2B.csproj", "{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RoboidControl", "src\RoboidControl.csproj", "{72BFCD03-FA78-4D0B-8B36-32301D60D6DD}"
EndProject
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "LinearAlgebra", "LinearAlgebra", "{3B5ED006-C6FC-4776-A6DC-FEE4E6909C03}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra", "LinearAlgebra\src\LinearAlgebra.csproj", "{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra_Test", "LinearAlgebra\test\LinearAlgebra_Test.csproj", "{8F286220-E70C-4E77-BDA6-6F28B726E320}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
Release|Any CPU = Release|Any CPU
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{B65BB41E-5A93-46FC-BA68-B865F50D57BD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{B65BB41E-5A93-46FC-BA68-B865F50D57BD}.Debug|Any CPU.Build.0 = Debug|Any CPU
{B65BB41E-5A93-46FC-BA68-B865F50D57BD}.Release|Any CPU.ActiveCfg = Release|Any CPU
{B65BB41E-5A93-46FC-BA68-B865F50D57BD}.Release|Any CPU.Build.0 = Release|Any CPU
{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9}.Debug|Any CPU.Build.0 = Debug|Any CPU
{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9}.Release|Any CPU.ActiveCfg = Release|Any CPU
{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9}.Release|Any CPU.Build.0 = Release|Any CPU
{72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Debug|Any CPU.Build.0 = Debug|Any CPU
{72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Release|Any CPU.ActiveCfg = Release|Any CPU
{72BFCD03-FA78-4D0B-8B36-32301D60D6DD}.Release|Any CPU.Build.0 = Release|Any CPU
{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Debug|Any CPU.Build.0 = Debug|Any CPU
{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Release|Any CPU.ActiveCfg = Release|Any CPU
{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3}.Release|Any CPU.Build.0 = Release|Any CPU
{8F286220-E70C-4E77-BDA6-6F28B726E320}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{8F286220-E70C-4E77-BDA6-6F28B726E320}.Debug|Any CPU.Build.0 = Debug|Any CPU
{8F286220-E70C-4E77-BDA6-6F28B726E320}.Release|Any CPU.ActiveCfg = Release|Any CPU
{8F286220-E70C-4E77-BDA6-6F28B726E320}.Release|Any CPU.Build.0 = Release|Any CPU
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
GlobalSection(NestedProjects) = preSolution
{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9} = {D6086F71-404B-4D18-BBE9-45947ED33DB2}
{CDCD9CE5-3224-4DB7-B7D6-5BB0714189B3} = {3B5ED006-C6FC-4776-A6DC-FEE4E6909C03}
{8F286220-E70C-4E77-BDA6-6F28B726E320} = {3B5ED006-C6FC-4776-A6DC-FEE4E6909C03}
EndGlobalSection
EndGlobal

25
Unity/DebugConsole.cs Normal file
View File

@ -0,0 +1,25 @@
#if UNITY_5_3_OR_NEWER
using System.IO;
using System.Text;
using UnityEngine;
namespace RoboidControl.Unity {
public class UnityLogWriter : TextWriter {
public override void Write(char value) {
Debug.Log(value);
}
public override void Write(string value) {
Debug.Log(value);
}
public override void WriteLine(string value) {
Debug.Log(value);
}
public override Encoding Encoding => Encoding.UTF8;
}
}
#endif

67
Unity/DistanceSensor.cs Normal file
View File

@ -0,0 +1,67 @@
#if UNITY_5_3_OR_NEWER
using System.Collections;
using UnityEngine;
namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a distance sensor
/// </summary>
public class DistanceSensor : Thing {
/// <summary>
/// The core distance sensor
/// </summary>
public new RoboidControl.DistanceSensor core {
get => (RoboidControl.DistanceSensor)base.core;
set => base.core = value;
}
/// <summary>
/// Start the Unity representation
/// </summary>
protected virtual void Start() {
if (core == null) {
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.DistanceSensor(siteServer.site));
}
StartCoroutine(MeasureDistance());
}
/// <summary>
/// Create the Unity representation of the distance sensor
/// </summary>
/// <param name="parent">The parent of the core distance sensor</param>
/// <returns>The Unity representation of the distance sensor</returns>
public static DistanceSensor Create(RoboidControl.DistanceSensor core) {
GameObject distanceObj = new("Distance sensor");
DistanceSensor component = distanceObj.AddComponent<DistanceSensor>();
if (core.parent != null && core.parent.component != null)
distanceObj.transform.SetParent(core.parent.component.transform, false);
return component;
}
/// <summary>
/// Periodically measure the distance
/// </summary>
/// <returns></returns>
IEnumerator MeasureDistance() {
while (Application.isPlaying) {
if (Physics.Raycast(this.transform.position, this.transform.forward, out RaycastHit hitInfo, 2.0f)) {
Thing thing = hitInfo.transform.GetComponentInParent<Thing>();
if (thing == null) {
// Debug.Log($"collision {hitInfo.transform.name} {hitInfo.distance}");
core.distance = hitInfo.distance;
}
else
core.distance = 0;
}
yield return new WaitForSeconds(0.1f);
}
}
}
}
#endif

39
Unity/SiteServer.cs Normal file
View File

@ -0,0 +1,39 @@
#if UNITY_5_3_OR_NEWER
using System;
using System.Collections.Generic;
using UnityEngine;
namespace RoboidControl.Unity {
public class SiteServer : MonoBehaviour {
public RoboidControl.SiteServer site;
public Queue<RoboidControl.Thing> thingQueue = new();
protected virtual void Awake() {
Console.SetOut(new UnityLogWriter());
site = new RoboidControl.SiteServer(7681);
RoboidControl.Thing.OnNewThing += HandleNewThing;
}
void OnApplicationQuit() {
if (site != null)
site.Close();
}
public void HandleNewThing(RoboidControl.Thing thing) {
//Debug.Log($"Handle New thing event for {thing}");
//site.Add(thing, false);
thingQueue.Enqueue(thing);
}
protected virtual void Update() {
site.Update((ulong)(Time.time * 1000));
while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
thing.CreateComponent();
}
}
}
#endif

146
Unity/Thing.cs Normal file
View File

@ -0,0 +1,146 @@
#if UNITY_5_3_OR_NEWER
using System.Collections;
using Mono.Cecil.Cil;
using UnityEngine;
using UnityEngine.Networking;
namespace RoboidControl.Unity {
/// <summary>
/// The representation of a Thing in Unity
/// </summary>
public class Thing : MonoBehaviour {
/// <summary>
/// The core C# thing
/// </summary>
[field: SerializeField]
public RoboidControl.Thing core { get; set; }
public SiteServer participant;
private string modelUrl = null;
/// <summary>
/// Set the core C# thing
/// </summary>
protected void SetCoreThing(RoboidControl.Thing thing) {
core = thing;
core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null || siteServer.site == null) {
Debug.LogWarning("No site server found");
return;
}
siteServer.site.Add(thing);
core.OnPoseChanged += PoseChanged;
}
public static Thing Create(RoboidControl.Thing core) {
// Debug.Log("Creating new Unity thing");
GameObject gameObj = string.IsNullOrEmpty(core.name) ?
new("Thing") :
new(core.name);
Thing component = gameObj.AddComponent<Thing>();
component.Init(core);
return component;
}
protected void Init(RoboidControl.Thing core) {
this.core = core;
this.participant = FindAnyObjectByType<SiteServer>();
if (core.parent != null && core.parent.component != null)
this.transform.SetParent(core.parent.component.transform, false);
if (core.position != null)
this.transform.localPosition = core.position.ToVector3();
if (core.orientation != null)
this.transform.localRotation = core.orientation.ToQuaternion();
core.OnPoseChanged += this.PoseChanged;
}
/// <summary>
/// Update the Unity representation
/// </summary>
protected virtual void Update() {
if (core == null) {
Debug.Log("Core thing is gone, self destruct in 0 seconds...");
Destroy(this);
return;
}
if (core.linearVelocity != null && core.linearVelocity.distance != 0) {
Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
this.transform.Translate(core.linearVelocity.distance * Time.deltaTime * direction, Space.Self);
}
if (core.angularVelocity != null && core.angularVelocity.distance != 0) {
Vector3 axis = core.angularVelocity.direction.ToVector3();
this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
}
if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf("."));
if (extension == ".jpg" || extension == ".png") {
StartCoroutine(LoadJPG());
}
this.modelUrl = core.modelUrl;
}
if (core.nameChanged) {
if (this.gameObject.name != core.name)
this.gameObject.name = core.name;
core.nameChanged = false;
}
if (core.hierarchyChanged) {
Debug.Log("Parent changed");
if (core.parent == null)
this.transform.SetParent(null, true);
else
this.transform.SetParent(core.parent.component.transform, true);
core.hierarchyChanged = false;
}
}
private void PoseChanged() {
//Debug.Log($"{this} pose changed");
if (core.positionUpdated)
this.transform.localPosition = core.position.ToVector3();
if (core.orientationUpdated)
this.transform.localRotation = core.orientation.ToQuaternion();
}
private IEnumerator LoadJPG() {
UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
yield return request.SendWebRequest();
if (request.result == UnityWebRequest.Result.Success) {
Texture2D texture = ((DownloadHandlerTexture)request.downloadHandler).texture;
float aspectRatio = (float)texture.width / (float)texture.height;
GameObject modelQuad = GameObject.CreatePrimitive(PrimitiveType.Quad);
Collider c = modelQuad.GetComponent<Collider>();
c.enabled = false;
Destroy(c);
modelQuad.transform.SetParent(this.transform, false);
modelQuad.transform.localEulerAngles = new(90, -90, 0);
modelQuad.transform.localScale = new Vector3(aspectRatio, 1, 1) / 5;
if (this.name == "Ant")
modelQuad.transform.localScale *= 2;
Material quadMaterial = new(Shader.Find("Unlit/Transparent")) {
mainTexture = texture
};
modelQuad.GetComponent<Renderer>().material = quadMaterial;
}
else {
Debug.LogError("Failed to load image: " + request.error);
}
}
}
}
#endif

74
Unity/TouchSensor.cs Normal file
View File

@ -0,0 +1,74 @@
#if UNITY_5_3_OR_NEWER
using UnityEngine;
namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of the TouchSensor
/// </summary>
public class TouchSensor : Thing {
// public SiteServer participant;
/// <summary>
/// The core touch sensor
/// </summary>
public RoboidControl.TouchSensor coreSensor {
get => (RoboidControl.TouchSensor)base.core;
}
/// <summary>
/// Start the Unity represention
/// </summary>
protected virtual void Start() {
if (core == null) {
participant = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.TouchSensor(participant.site));
}
}
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns>
public static TouchSensor Create(RoboidControl.TouchSensor core) {
GameObject gameObj = core.name != null ?
new(core.name) :
new("Touch Sensor");
TouchSensor component = gameObj.AddComponent<TouchSensor>();
component.Init(core);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.01f;
collider.isTrigger = true;
if (gameObj.transform.parent != null && gameObj.transform.localPosition.magnitude > 0) {
collider.radius = Vector3.Distance(gameObj.transform.position, gameObj.transform.parent.position) / 2;
gameObj.transform.position = (gameObj.transform.position + gameObj.transform.parent.position) / 2;
}
return component;
}
private void OnTriggerEnter(Collider other) {
if (other.isTrigger)
return;
if (this.transform.root == other.transform.root)
return;
Debug.Log($"*** {this} Touch");
this.coreSensor.touchedSomething = true;
}
private void OnTriggerExit(Collider other) {
if (other.isTrigger)
return;
Debug.Log($"*** {this} Touch end");
this.coreSensor.touchedSomething = false;
}
}
}
#endif

78
src/Messages/BinaryMsg.cs Normal file
View File

@ -0,0 +1,78 @@
namespace RoboidControl {
/// <summary>
/// A message containing binary data for custom communication
/// </summary>
public class BinaryMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xB1;
/// <summary>
/// The length of the message, excluding the binary data
/// </summary>
/// For the total size of the message this.bytes.Length should be added to this value.
public const byte length = 4;
/// <summary>
/// The network ID identifying the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
public Thing thing;
/// <summary>
/// The length of the data
/// </summary>
public byte dataLength;
/// <summary>
/// The binary data
/// </summary>
public byte[] data;
/// <summary>
/// Create an empty message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
public BinaryMsg(byte networkId, Thing thing) : base() {
this.networkId = networkId;
this.thingId = thing.id;
this.thing = thing;
this.data = this.thing.GenerateBinary();
this.dataLength = (byte)this.data.Length;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.dataLength = buffer[ix++];
this.data = new byte[this.dataLength];
for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
this.data[dataIx] = buffer[ix++];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < BinaryMsg.length + this.data.Length || this.data.Length == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
#endif
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.dataLength;
foreach (byte b in data)
buffer[ix++] = b;
return ix;
}
}
}

54
src/Messages/DestroyMsg.cs Normal file
View File

@ -0,0 +1,54 @@
namespace RoboidControl {
/// <summary>
/// Message notifiying that a Thing no longer exists
/// </summary>
public class DestroyMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x20;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 3;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// Create a message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
public DestroyMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public DestroyMsg(byte[] buffer) : base(buffer) {
this.networkId = buffer[1];
this.thingId = buffer[2];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < DestroyMsg.length)
return 0;
byte ix = 0;
buffer[ix++] = DestroyMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
}
}

46
src/Messages/InvestigateMsg.cs Normal file
View File

@ -0,0 +1,46 @@
namespace RoboidControl {
/// <summary>
/// Message to request details for a Thing
/// </summary>
public class InvestigateMsg : IMessage {
/// <summary>
/// The message Id
/// </summary>
public const byte Id = 0x81;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 3;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID for the thing</param>
/// <param name="thingId">The ID of the thing</param>
public InvestigateMsg(byte networkId, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public InvestigateMsg(byte[] buffer) : base(buffer) { }
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = InvestigateMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
}
}

98
src/Messages/LowLevelMessages.cs Normal file
View File

@ -0,0 +1,98 @@
using LinearAlgebra;
namespace RoboidControl {
public class LowLevelMessages {
public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) {
SendFloat16(buffer, ref ix, new float16(v.distance));
SendAngle8(buffer, ref ix, v.direction.horizontal);
SendAngle8(buffer, ref ix, v.direction.vertical);
}
public static Spherical ReceiveSpherical(byte[] data, ref byte ix) {
float distance = ReceiveFloat16(data, ref ix);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
Spherical v = Spherical.Degrees(distance, horizontal, vertical);
return v;
}
public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
Quat32 q32 = Quat32.FromSwingTwist(s);
SendQuat32(buffer, ref ix, q32);
}
public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) {
int qx = (int)(q.x * 127 + 128);
int qy = (int)(q.y * 127 + 128);
int qz = (int)(q.z * 127 + 128);
int qw = (int)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
buffer[ix++] = (byte)qx;
buffer[ix++] = (byte)qy;
buffer[ix++] = (byte)qz;
buffer[ix++] = (byte)qw;
}
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) {
Quat32 q = new Quat32(
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
data[ix++] / 255.0F);
return q;
}
public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
Quat32 q32 = ReceiveQuat32(data, ref ix);
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
SwingTwist r = SwingTwist.FromQuat32(q32);
return r;
}
public static void SendAngle8(byte[] buffer, ref byte ix, float angle) {
// Normalize angle
while (angle >= 180)
angle -= 360;
while (angle < -180)
angle += 360;
sbyte value = (sbyte)(angle / 360.0f * 256.0f);
buffer[ix++] = (byte)value;
}
public static float ReceiveAngle8(byte[] data, ref byte ix) {
float value = (data[ix++] * 180) / 128.0F;
return value;
}
public static void SendFloat16(byte[] data, ref byte ix, float f) {
float16 f16 = new float16(f);
ushort binary = f16.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
public static void SendFloat16(byte[] data, ref byte ix, float16 f) {
ushort binary = f.GetBinary();
data[ix++] = (byte)((binary >> 8) & 0xFF);
data[ix++] = (byte)(binary & 0xFF);
}
public static float ReceiveFloat16(byte[] data, ref byte ix) {
byte msb = data[ix++];
byte lsb = data[ix++];
ushort value = (ushort)(msb << 8 | lsb);
float16 f16 = new float16();
f16.SetBinary(value);
float f = f16.toFloat();
return f;
}
}
}

25
src/Messages/Messages.cs Normal file
View File

@ -0,0 +1,25 @@
namespace RoboidControl {
/// <summary>
/// Root structure for all communcation messages
/// </summary>
public class IMessage {
public IMessage() { }
/// <summary>
/// Create a message for receiving
/// </summary>
/// <param name="buffer">The byte array to parse</param>
public IMessage(byte[] buffer) {
//Deserialize(buffer);
}
/// <summary>
/// Serialize the message into a byte array for sending
/// </summary>
/// <param name="buffer">The buffer to serilize into</param>
/// <returns>The length of the message in the buffer</returns>
public virtual byte Serialize(ref byte[] buffer) { return 0; }
}
}

86
src/Messages/ModelUrlMsg.cs Normal file
View File

@ -0,0 +1,86 @@
namespace RoboidControl {
/// <summary>
/// Message for communicating the URL for a model of the thing
/// </summary>
public class ModelUrlMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x90; // (144) Model URL
/// <summary>
/// The length of the message without th URL itself
/// </summary>
public const byte length = 4;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The length of the url string, excluding the null terminator
/// </summary>
public byte urlLength;
/// <summary>
/// The URL of the model
/// </summary>
public string url = null;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing for which to send the model URL</param>
public ModelUrlMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.urlLength = (byte)thing.modelUrl.Length;
this.url = thing.modelUrl;
}
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="url">The URL to send</param>
// public ModelUrlMsg(byte networkId, byte thingId, string url) {
// this.networkId = networkId;
// this.thingId = thingId;
// this.urlLength = (byte)url.Length;
// this.url = url;
// }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public ModelUrlMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.urlLength = buffer[ix++];
this.url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.urlLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (string.IsNullOrEmpty(this.url))
return 0;
#if DEBUG
System.Console.WriteLine($"Send ModelUrlMsg [{this.networkId}/{this.thingId}] {this.urlLength} {this.url}");
#endif
byte ix = 0;
buffer[ix++] = ModelUrlMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; // Thing Id
buffer[ix++] = (byte)this.url.Length;
for (int urlIx = 0; urlIx < this.url.Length; urlIx++)
buffer[ix++] = (byte)url[urlIx];
return ix;
}
}
}

84
src/Messages/NameMsg.cs Normal file
View File

@ -0,0 +1,84 @@
namespace RoboidControl {
/// <summary>
/// Message for communicating the name of a thing
/// </summary>
public class NameMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x91; // 145
/// <summary>
/// The length of the message without the name string
/// </summary>
public const byte length = 4;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The length of the name, excluding null terminator
/// </summary>
public byte nameLength;
/// <summary>
/// The name of the thing, not terminated with a null character
/// </summary>
public string name = "";
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing</param>
public NameMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.name = thing.name;
this.nameLength = (byte)this.name.Length;
}
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="name">The name of the thing</param>
// public NameMsg(byte networkId, byte thingId, string name) {
// this.networkId = networkId;
// this.thingId = thingId;
// this.name = name;
// }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public NameMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.nameLength = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.nameLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < NameMsg.length + this.name.Length || string.IsNullOrEmpty(this.name))
return 0;
#if DEBUG
System.Console.WriteLine($"Send NameMsg [{this.networkId}/{this.thingId}] {this.nameLength} {this.name}");
#endif
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.nameLength;
for (int nameIx = 0; nameIx < this.nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;
}
}
}

46
src/Messages/NetworkIdMsg.cs Normal file
View File

@ -0,0 +1,46 @@
namespace RoboidControl {
/// <summary>
/// A message communicating the network ID for that participant
/// </summary>
public class NetworkIdMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xA1;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 2;
/// <summary>
/// The network ID for the participant
/// </summary>
public byte networkId;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID for the participant</param>
public NetworkIdMsg(byte networkId) {
this.networkId = networkId;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public NetworkIdMsg(byte[] buffer) {
this.networkId = buffer[1];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < NetworkIdMsg.length)
return 0;
#if DEBUG
System.Console.WriteLine($"Send NetworkIdMsg {this.networkId}");
#endif
buffer[0] = NetworkIdMsg.Id;
buffer[1] = this.networkId;
return NetworkIdMsg.length;
}
}
}

53
src/Messages/ParticipantMsg.cs Normal file
View File

@ -0,0 +1,53 @@
namespace RoboidControl {
/// <summary>
/// A participant messages notifies other participants of its presence
/// </summary>
/// When received by another participant, it can be followed by a NetworkIdMsg
/// to announce that participant to this client such that it can join privately
public class ParticipantMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xA0;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 2;
/// <summary>
/// The network ID known by the participant
/// </summary>
public byte networkId;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID known by the participant</param>
public ParticipantMsg(byte networkId) {
this.networkId = networkId;
}
/// <summary>
/// Create a message for receiving
/// </summary>
/// <param name="buffer">The byte array to parse</param>
public ParticipantMsg(byte[] buffer) {
this.networkId = buffer[1];
}
/// <summary>
/// Serialize the message into a byte array
/// </summary>
/// <param name="buffer">The buffer to serialize into</param>
/// <returns>The length of the message in the buffer</returns>
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < ParticipantMsg.length)
return 0;
byte ix = 0;
buffer[ix++] = ParticipantMsg.Id;
buffer[ix++] = networkId;
return ParticipantMsg.length;
}
}
}

165
src/Messages/PoseMsg.cs Normal file
View File

@ -0,0 +1,165 @@
using LinearAlgebra;
namespace RoboidControl {
/// <summary>
/// Message to communicate the pose of the thing
/// </summary>
/// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will be in world space.
public class PoseMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0x10;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 4 + 4 + 4;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// Bit pattern stating which pose components are available
/// </summary>
public byte poseType;
/// <summary>
/// Bit pattern for a pose with position
/// </summary>
public const byte Pose_Position = 0x01;
/// <summary>
/// Bit pattern for a pose with orientation
/// </summary>
public const byte Pose_Orientation = 0x02;
/// <summary>
/// Bit pattern for a pose with linear velocity
/// </summary>
public const byte Pose_LinearVelocity = 0x04;
/// <summary>
/// Bit pattern for a pose with angular velocity
/// </summary>
public const byte Pose_AngularVelocity = 0x08;
/// <summary>
/// The position of the thing in local space in meters
/// </summary>
public Spherical position = Spherical.zero;
/// <summary>
/// The orientation of the thing in local space
/// </summary>
public SwingTwist orientation = SwingTwist.zero;
/// <summary>
/// The linear velocity of the thing in local space in meters per second
/// </summary>
public Spherical linearVelocity = Spherical.zero;
/// <summary>
/// The angular velocity of the thing in local space
/// </summary>
public Spherical angularVelocity = Spherical.zero;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="position">The position of the thing in local space in meters</param>
/// <param name="orientation">The orientation of the thing in local space</param>
public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation, Spherical linearVelocity = null, Spherical angularVelocity = null) {
this.networkId = networkId;
this.thingId = thingId;
this.poseType = 0;
if (this.position != null)
this.poseType |= Pose_Position;
if (this.orientation != null)
this.poseType |= Pose_Orientation;
if (this.linearVelocity != null)
this.poseType |= Pose_LinearVelocity;
if (this.angularVelocity != null)
this.poseType |= Pose_AngularVelocity;
this.position = position;
this.orientation = orientation;
this.linearVelocity = linearVelocity;
this.angularVelocity = angularVelocity;
}
public PoseMsg(byte networkId, Thing thing, bool force = false) {
this.networkId = networkId;
this.thingId = thing.id;
this.poseType = 0;
if (thing.positionUpdated || force) {
this.position = thing.position;
this.poseType |= Pose_Position;
}
if (thing.orientationUpdated || force) {
this.orientation = thing.orientation;
this.poseType |= Pose_Orientation;
}
if (thing.linearVelocityUpdated) {
this.linearVelocity = thing.linearVelocity;
this.poseType |= Pose_LinearVelocity;
thing.linearVelocityUpdated = false;
}
if (thing.angularVelocityUpdated) {
this.angularVelocity = thing.angularVelocity;
this.poseType |= Pose_AngularVelocity;
thing.angularVelocityUpdated = false;
}
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public PoseMsg(byte[] buffer) : base(buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.poseType = buffer[ix++];
this.position = null;
this.orientation = null;
this.linearVelocity = null;
this.angularVelocity = null;
if ((this.poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_Orientation) != 0)
this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
if ((this.poseType & Pose_LinearVelocity) != 0)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((this.poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (poseType == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send PoseMsg [{this.networkId}/{this.thingId}] {this.poseType}");
#endif
byte ix = 0;
buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
if ((poseType & Pose_Orientation) != 0)
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
if ((poseType & Pose_LinearVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
if ((poseType & Pose_AngularVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
return ix;
}
}
}

56
src/Messages/TextMsg.cs Normal file
View File

@ -0,0 +1,56 @@
namespace RoboidControl {
/// <summary>
/// Message for sending generic text
/// </summary>
public class TextMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte Id = 0xB0;
/// <summary>
/// The length of the message without the text itself
/// </summary>
public const byte length = 2;
/// <summary>
/// The length of the text without the null terminator
/// </summary>
public byte textLength;
/// <summary>
/// The text
/// </summary>
public string text = "";
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="text">The text to send</param>
public TextMsg(string text) {
this.textLength = (byte)text.Length;
this.text = text;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public TextMsg(byte[] buffer) : base(buffer) {
this.textLength = buffer[0];
this.text = System.Text.Encoding.UTF8.GetString(buffer, 1, this.textLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < TextMsg.length + this.text.Length || this.text.Length == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send TextMsg {this.textLength} {this.text}");
#endif
byte ix = 0;
buffer[ix++] = TextMsg.Id;
buffer[ix++] = this.textLength;
for (int textIx = 0; textIx < this.text.Length; textIx++)
buffer[ix++] = (byte)this.text[textIx];
return ix;
}
}
}

93
src/Messages/ThingMsg.cs Normal file
View File

@ -0,0 +1,93 @@
namespace RoboidControl {
/// <summary>
/// Message providing generic information about a Thing
/// </summary>
public class ThingMsg : IMessage {
/// <summary>
/// The message ID
/// </summary>
public const byte id = 0x80;
/// <summary>
/// The length of the message
/// </summary>
public const byte length = 5;
/// <summary>
/// The network ID of the thing
/// </summary>
public byte networkId;
/// <summary>
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The Thing.Type of the thing
/// </summary>
public byte thingType;
/// <summary>
/// The parent of the thing in the hierarachy. This is null for root Things
/// </summary>
public byte parentId;
/// <summary>
/// Create a message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thing">The thing</param>
public ThingMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.thingType = thing.type;
if (thing.parent != null)
this.parentId = thing.parent.id;
else
this.parentId = 0;
}
/// <summary>
/// Create a message for sending
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="thingType">The type of thing</param>
/// <param name="parentId">The parent of the thing</param>
public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) {
this.networkId = networkId;
this.thingId = thingId;
this.thingType = thingType;
this.parentId = parentId;
}
/// <summary>
/// Create a message for receiving
/// </summary>
/// <param name="buffer">The byte array to parse</param>
public ThingMsg(byte[] buffer) {
uint ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.thingType = buffer[ix++];
this.parentId = buffer[ix];
}
/// <summary>
/// Serialize the message into a byte array
/// </summary>
/// <param name="buffer">The buffer to serialize into</param>
/// <returns>The length of the message in the bufer. 0 when the buffer was too small</returns>
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < ThingMsg.length)
return 0;
#if DEBUG
System.Console.WriteLine($"Send ThingMsg [{this.networkId}/{this.thingId}] {this.thingType} {this.parentId}");
#endif
byte ix = 0;
buffer[ix++] = ThingMsg.id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.thingType;
buffer[ix++] = this.parentId;
return ThingMsg.length;
}
}
}

143
src/Participant.cs Normal file
View File

@ -0,0 +1,143 @@
using System;
using System.Collections.Generic;
namespace RoboidControl {
/// <summary>
/// A participant is a device which manages things.
/// </summary>
/// It can communicate with other participant to synchronise the state of things.
/// This class is used to register the things the participant is managing.
/// It also maintains the communcation information to contact the participant.
/// It is used as a basis for the local participant, but also as a reference to remote participants.
public class Participant {
/// <summary>
/// Default constructor
/// </summary>
public Participant() { }
/// <summary>
/// Create a new participant with the given communcation info
/// </summary>
/// <param name="ipAddress">The IP address of the participant</param>
/// <param name="port">The UDP port of the participant</param>
public Participant(string ipAddress, int port) {
this.ipAddress = ipAddress;
this.port = port;
}
/// <summary>
/// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
/// </summary>
public string ipAddress = "0.0.0.0";
/// <summary>
/// The port number for UDP communication with the participant. This is 0 for isolated participants.
/// </summary>
public int port = 0;
/// <summary>
/// The network ID of the participant
/// </summary>
public byte networkId;
/// <summary>
/// The things managed by this participant
/// </summary>
public readonly List<Thing> things = new List<Thing>();
public virtual void Update(ulong currentTimeMS = 0) {
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing != null)
thing.Update(currentTimeMS, true);
}
}
public static List<Participant> participants = new List<Participant>();
public static Participant GetParticipant(string ipAddress, int port) {
//Console.WriteLine($"Get Participant {ipAddress}:{port}");
foreach (Participant participant in Participant.participants) {
if (participant.ipAddress == ipAddress && participant.port == port)
return participant;
}
return null;
}
public static Participant GetParticipant(int participantId) {
//Console.WriteLine($"Get Participant [participantId]");
foreach (Participant participant in Participant.participants) {
if (participant.networkId == participantId)
return participant;
}
return null;
}
public static Participant AddParticipant(string ipAddress, int port) {
Console.WriteLine($"New Participant {ipAddress}:{port}");
Participant participant = new(ipAddress, port) {
networkId = (byte)(Participant.participants.Count + 1)
};
Participant.participants.Add(participant);
return participant;
}
public static void AddParticipant(Participant participant) {
Participant foundParticipant = Participant.GetParticipant(participant.networkId);
if (foundParticipant == null)
Participant.participants.Add(participant);
}
/// <summary>
/// Get a thing with the given ids
/// </summary>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <returns>The thing when it is found, null in other cases.</returns>
public Thing Get(byte networkId, byte thingId) {
Thing thing = things.Find(aThing => aThing.id == thingId);
//Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId));
// if (thing == null)
// Console.WriteLine($"Could not find thing {ipAddress}:{port}[{networkId}/{thingId}]");
return thing;
}
/// <summary>
/// Add a new thing for this participant
/// </summary>
/// <param name="thing">The thing to add</param>
/// <param name="invokeEvent">Invoke an notification event when the thing has been added</param>
public void Add(Thing thing, bool checkId = true, bool invokeEvent = true) {
if (checkId && thing.id == 0) {
thing.id = (byte)(this.things.Count + 1);
this.things.Add(thing);
}
// Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]");
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
this.things.Add(thing);
// if (invokeEvent)
// Thing.InvokeNewThing(thing);
// Console.Write($"Add thing {ipAddress}:{port}[{networkId}/{thing.id}]");
}
// else {
// if (thing != foundThing) {
// // should be: find first non-existing id...
// thing.id = (byte)this.things.Count;
// things.Add(thing);
// // Console.Write($"Add thing, updated thing id to [{thing.networkId}/{thing.id}]");
// }
// }
}
/// <summary>
/// Remove a thing for this participant
/// </summary>
/// <param name="thing">The thing to remove</param>
public void Remove(Thing thing) {
this.things.Remove(thing);
}
}
}

417
src/Participants/ParticipantUDP.cs Normal file
View File

@ -0,0 +1,417 @@
using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Net;
using System.Net.Sockets;
namespace RoboidControl {
/// <summary>
/// A participant is used for communcation between things
/// </summary>
public class ParticipantUDP : Participant {
public byte[] buffer = new byte[1024];
public ulong publishInterval = 3000; // = 3 seconds
public string name = "Participant";
public bool isIsolated = false;
public Participant remoteSite;
public IPEndPoint endPoint = null;
public UdpClient udpClient = null;
public string broadcastIpAddress = "255.255.255.255";
public readonly ConcurrentQueue<IMessage> messageQueue = new ConcurrentQueue<IMessage>();
#region Init
/// <summary>
/// Create a participant with the give UDP port
/// </summary>
/// <param name="port">The port number on which to communicate</param>
public ParticipantUDP(int port = 0) : base("127.0.0.1", port) {
if (this.port == 0)
this.isIsolated = true;
Participant.AddParticipant(this);
}
/// <summary>
/// Create a new participant for a site at the given address and port
/// </summary>
/// <param name="ipAddress">The ip address of the site server</param>
/// <param name="port">The port number of the site server</param>
public ParticipantUDP(string ipAddress, int port = 7681, int localPort = 7681) : base("127.0.0.1", localPort) {
if (this.port == 0)
this.isIsolated = true;
else
this.remoteSite = new Participant(ipAddress, port);
Participant.AddParticipant(this);
this.endPoint = new IPEndPoint(IPAddress.Any, localPort);
this.udpClient = new UdpClient(localPort);
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
}
/// <summary>
/// Create a participant using the given udp client
/// </summary>
/// <param name="udpClient">UDP client to use for communication</param>
/// <param name="port">The port number on which to communicate</param>
public ParticipantUDP(UdpClient udpClient, int port) : this() {
this.udpClient = udpClient;
this.port = port;
}
private static ParticipantUDP isolatedParticipant = null;
public static ParticipantUDP Isolated() {
if (isolatedParticipant == null)
isolatedParticipant = new ParticipantUDP(0);
return isolatedParticipant;
}
// public List<Participant> owners = new List<Participant>();
// public Participant GetParticipant(string ipAddress, int port) {
// //Console.WriteLine($"Get Participant {ipAddress}:{port}");
// foreach (Participant sender in owners) {
// if (sender.ipAddress == ipAddress && sender.port == port)
// return sender;
// }
// return null;
// }
// public Participant AddParticipant(string ipAddress, int port) {
// Console.WriteLine($"New Participant {ipAddress}:{port}");
// Participant participant = new(ipAddress, port) {
// networkId = (byte)(this.owners.Count + 1)
// };
// owners.Add(participant);
// return participant;
// }
// protected readonly Dictionary<byte, Func<Participant, byte, byte, Thing>> thingMsgProcessors = new();
#endregion Init
#region Update
protected ulong nextPublishMe = 0;
public override void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0)
currentTimeMS = Thing.GetTimeMs();
if (this.isIsolated == false) {
if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) {
ParticipantMsg msg = new ParticipantMsg(this.networkId);
if (this.remoteSite == null)
this.Publish(msg);
else
this.Send(this.remoteSite, msg);
this.nextPublishMe = currentTimeMS + this.publishInterval;
}
}
UpdateMyThings(currentTimeMS);
UpdateOtherThings(currentTimeMS);
}
protected virtual void UpdateMyThings(ulong currentTimeMS) {
foreach (Thing thing in this.things) {
if (thing == null)
continue;
if (thing.hierarchyChanged && !(this.isIsolated || this.networkId == 0)) {
ThingMsg thingMsg = new(this.networkId, thing);
this.Send(this.remoteSite, thingMsg);
}
// Why don't we do recursive?
// Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated!
thing.Update(currentTimeMS, false);
if (!(this.isIsolated || this.networkId == 0)) {
if (thing.terminate) {
DestroyMsg destroyMsg = new(this.networkId, thing);
this.Send(this.remoteSite, destroyMsg);
}
else {
// Send to remote site
PoseMsg poseMsg = new(thing.owner.networkId, thing);
this.Send(this.remoteSite, poseMsg);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
this.Send(this.remoteSite, binaryMsg);
}
}
if (thing.terminate)
this.Remove(thing);
}
}
protected virtual void UpdateOtherThings(ulong currentTimeMS) {
for (int ownerIx = 0; ownerIx < Participant.participants.Count; ownerIx++) {
Participant participant = Participant.participants[ownerIx];
if (participant == null || participant == this)
continue;
participant.Update(currentTimeMS);
if (this.isIsolated)
continue;
//foreach (Thing thing in participant.things) {
for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
Thing thing = participant.things[thingIx];
if (thing == null)
continue;
PoseMsg poseMsg = new(thing.owner.networkId, thing);
this.Send(participant, poseMsg);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
this.Send(participant, binaryMsg);
}
}
}
#endregion Update
#region Send
public void SendThingInfo(Participant owner, Thing thing) {
// Console.WriteLine("Send thing info");
this.Send(owner, new ThingMsg(this.networkId, thing));
this.Send(owner, new NameMsg(this.networkId, thing));
this.Send(owner, new ModelUrlMsg(this.networkId, thing));
this.Send(owner, new BinaryMsg(this.networkId, thing));
}
public bool Send(Participant owner, IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(owner.ipAddress), owner.port);
// Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
return true;
}
public void PublishThingInfo(Thing thing) {
// Console.WriteLine("Publish thing info");
this.Publish(new ThingMsg(this.networkId, thing));
this.Publish(new NameMsg(this.networkId, thing));
this.Publish(new ModelUrlMsg(this.networkId, thing));
this.Publish(new BinaryMsg(this.networkId, thing));
}
public bool Publish(IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
// public bool SendBuffer(int bufferSize) {
// //if (this.ipAddress == null)
// // return false;
// // UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
// //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port);
// this.udpClient?.Send(this.buffer, bufferSize, this.endPoint);
// return true;
// }
// public bool PublishBuffer(int bufferSize) {
// if (this.broadcastIpAddress == null)
// return false;
// this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
// return true;
// }
#endregion
#region Receive
protected void ReceiveUDP(IAsyncResult result) {
// UnityEngine.Debug.Log("received");
if (this.udpClient == null) // || this.endPoint == null)
return;
byte[] data = this.udpClient.EndReceive(result, ref endPoint);
// This does not yet take multi-packet messages into account!
if (endPoint == null)
return;
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
string ipAddress = endPoint.Address.ToString();
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port);
ReceiveData(data, remoteParticipant);
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
public void ReceiveData(byte[] data, Participant sender) {
byte msgId = data[0];
if (msgId == 0xFF) {
// Timeout
return;
}
switch (msgId) {
case ParticipantMsg.Id: // 0xA0 / 160
this.Process(sender, new ParticipantMsg(data));
break;
case NetworkIdMsg.Id: // 0xA1 / 161
this.Process(sender, new NetworkIdMsg(data));
break;
case InvestigateMsg.Id: // 0x81
// result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.id: // 0x80 / 128
this.Process(sender, new ThingMsg(data));
break;
case NameMsg.Id: // 0x91 / 145
this.Process(sender, new NameMsg(data));
break;
case ModelUrlMsg.Id: // 0x90 / 144
this.Process(sender, new ModelUrlMsg(data));
break;
case PoseMsg.Id: // 0x10 / 16
this.Process(sender, new PoseMsg(data));
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case BinaryMsg.Id: // 0xB1 / 177
this.Process(sender, new BinaryMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
break;
case DestroyMsg.Id: // 0x20 / 32
this.Process(sender, new DestroyMsg(data));
// result = await DestroyMsg.Receive(dataStream, client, packetSize);
break;
default:
break;
}
}
protected virtual void Process(Participant sender, ParticipantMsg msg) {
#if DEBUG
Console.WriteLine($"{this.name} Process participantMsg {msg.networkId}");
#endif
}
protected virtual void Process(Participant sender, NetworkIdMsg msg) {
#if DEBUG
Console.WriteLine($"{this.name} Process SiteMsg {this.networkId} -> {msg.networkId}");
#endif
if (this.networkId != msg.networkId) {
this.networkId = msg.networkId;
foreach (Thing thing in this.things) //Thing.GetAllThings())
this.SendThingInfo(sender, thing);
}
}
protected virtual void Process(Participant sender, InvestigateMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: InvestigateMsg [{msg.networkId}/{msg.thingId}]");
#endif
}
protected virtual void Process(Participant sender, ThingMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId}");
#endif
}
protected virtual void Process(Participant sender, NameMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process NameMsg [{msg.networkId}/{msg.thingId}] {msg.nameLength} {msg.name}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.name = msg.name;
}
protected virtual void Process(Participant sender, ModelUrlMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process ModelUrlMsg [{msg.networkId}/{msg.thingId}] {msg.urlLength} {msg.url}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.modelUrl = msg.url;
}
protected virtual void Process(Participant sender, PoseMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) {
if ((msg.poseType & PoseMsg.Pose_Position) != 0)
thing.position = msg.position;
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
thing.orientation = msg.orientation;
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0) {
thing.linearVelocity = msg.linearVelocity;
//Console.Write($"linear velocity = {thing.linearVelocity.ToVector3()}");
}
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) {
thing.angularVelocity = msg.angularVelocity;
//Console.Write($"angular velocity = {thing.angularVelocity.ToVector3()}");
}
}
}
protected virtual void Process(Participant sender, BinaryMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.data);
}
protected virtual void Process(Participant sender, TextMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process TextMsg {msg.textLength} {msg.text}");
#endif
}
protected virtual void Process(Participant sender, DestroyMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process Destroy Msg [{msg.networkId}/{msg.thingId}]");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
this.Remove(thing);
thing.component.core = null;
}
private void ForwardMessage(IMessage msg) {
// foreach (Participant client in senders) {
// if (client == this)
// continue;
// //UnityEngine.Debug.Log($"---> {client.ipAddress}");
// //IMessage.SendMsg(client, msg);
// msg.Serialize(ref client.buffer);
// client.SendBuffer(client.buffer.Length);
// }
}
#endregion
}
}

97
src/Participants/SiteServer.cs Normal file
View File

@ -0,0 +1,97 @@
using System;
using System.Net;
using System.Net.Sockets;
namespace RoboidControl {
/// <summary>
/// A site server is a participant which provides a shared simulated environment
/// </summary>
public class SiteServer : ParticipantUDP {
/// <summary>
/// Create a new site server
/// </summary>
/// <param name="port"></param>
public SiteServer(int port = 7681) : base(port) {
this.name = "Site Server";
Participant.AddParticipant(this);
Console.Write($"Prepare receive on port {port}");
this.endPoint = new IPEndPoint(IPAddress.Any, port);
this.udpClient = new UdpClient(port);
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
}
/// <summary>
/// Close the site
/// </summary>
public void Close() {
this.udpClient?.Close();
}
#region Update
protected override void UpdateMyThings(ulong currentTimeMS) {
// We don't use foreach to prevent the 'Collection was modified' error
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing == null)
continue;
thing.Update(currentTimeMS, false);
if (this.isIsolated == false) {
// Send to all other participants
foreach (Participant participant in Participant.participants) {
if (participant == null || participant == this)
continue;
PoseMsg poseMsg = new(thing.owner.networkId, thing);
this.Send(participant, poseMsg);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
this.Send(participant, binaryMsg);
}
}
}
}
#endregion Update
#region Receive
protected override void Process(Participant sender, ParticipantMsg msg) {
base.Process(sender, msg);
if (msg.networkId != sender.networkId) {
//Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
this.Send(sender, new NetworkIdMsg(sender.networkId));
}
}
protected override void Process(Participant sender, NetworkIdMsg msg) { }
protected override void Process(Participant sender, ThingMsg msg) {
Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} ");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null)
thing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
if (msg.parentId != 0) {
thing.parent = sender.Get(msg.networkId, msg.parentId);
if (thing.parent == null)
Console.WriteLine($"Could not find parent [{msg.networkId}/{msg.parentId}]");
}
else {
Console.Write($"Dropped {thing.id}");
thing.parent = null;
}
}
#endregion Receive
}
}

14
src/RoboidControl.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" />
<ProjectReference Include="..\LinearAlgebra\src\LinearAlgebra.csproj" />
</ItemGroup>
</Project>

438
src/Thing.cs Normal file
View File

@ -0,0 +1,438 @@
using System;
using System.Collections.Generic;
using LinearAlgebra;
namespace RoboidControl {
/// <summary>
/// A thing is the primitive building block
/// </summary>
[Serializable]
public class Thing {
#region Types
/// <summary>
/// Predefined thing types
/// </summary>
public enum Type {
Undetermined,
// Sensor
Switch,
DistanceSensor,
DirectionalSensor,
TemperatureSensor,
TouchSensor,
// Motor
ControlledMotor,
UncontrolledMotor,
Servo,
// Other
Roboid,
Humanoid,
ExternalSensor
};
public delegate void ChangeHandler();
public delegate void SphericalHandler(Spherical v);
public delegate void ThingHandler(Thing t);
#endregion Types
#region Init
/// <summary>
/// Create a new thing for a participant
/// </summary>
/// <param name="owner">The participant owning the thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
this.owner = owner;
this.type = thingType;
if (this.owner != null)
this.owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
}
public Thing(Participant owner) : this(owner, Type.Undetermined) { }
/// <summary>
/// Create a new thing for a participant
/// </summary>
/// <param name="owner">The participant owning the thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(Participant owner, Type thingType = Type.Undetermined, bool invokeEvent = true) : this(owner, (byte)thingType, invokeEvent) {
}
/// <summary>
/// Create a new thing without communication abilities
/// </summary>
/// <param name="thingType">The type of thing</param>
public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, invokeEvent) {
}
/// <summary>
/// Create a new thing as a child of another thing
/// </summary>
/// <param name="parent">The parent thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(Thing parent, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(parent.owner, thingType, invokeEvent) {
this.parent = parent;
}
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="owner">The participant owning the thing</param>
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="thingType">The type of thing</param>
public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) {
this.owner = owner;
this.id = thingId;
this.type = thingType;
this.networkId = networkId;
Console.Write($"New thing added to {owner}");
this.owner.Add(this);
InvokeNewThing(this);
}
/// <summary>
/// Function which can be used to create components in external engines.
/// </summary>
/// Currently this is used to create GameObjects in Unity
public virtual void CreateComponent() {
#if UNITY_5_3_OR_NEWER
this.component = Unity.Thing.Create(this);
this.component.core = this;
#endif
}
#endregion Init
#region Properties
/// <summary>
/// The participant owning this thing
/// </summary>
public Participant owner = null;
/// <summary>
/// The network ID of this thing.
/// </summary>
/// @note This field will likely disappear in future versions
public byte networkId = 0;
/// <summary>
/// The ID of this thing
/// </summary>
public byte id = 0;
/// <summary>
/// The type of this thing.
/// </summary>
/// This can be either a Thing::Type (needs casting) or a byte value for custom types.
public byte type = (byte)Type.Undetermined;
private Thing _parent;
/// <summary>
/// The parent of this thing
/// </summary>
public Thing parent {
get => _parent;
set {
if (_parent == value)
return;
if (value == null) {
_parent?.RemoveChild(this);
_parent = null;
}
else {
value.AddChild(this);
}
this.hierarchyChanged = true;
}
}
/// <summary>
/// Add a child Thing to this Thing
/// </summary>
/// <param name="child">The Thing which should become a child</param>
/// @remark When the Thing is already a child, it will not be added again
public void AddChild(Thing child) {
if (children.Find(thing => thing == child) != null)
return;
child._parent = this;
children.Add(child);
}
/// <summary>
/// Remove the given thing as a child of this thing
/// </summary>
/// <param name="child">The child to remove</param>
/// <returns>True when the child was present or false when it was not found</returns>
public bool RemoveChild(Thing child) {
return children.Remove(child);
}
/// <summary>
/// Get a child by thing Id
/// </summary>
/// <param name="thingId"></param>
/// <param name="recursively"></param>
/// <returns></returns>
Thing GetChild(byte thingId, bool recursively = false) {
foreach (Thing child in this.children) {
if (child == null)
continue;
if (child.id == thingId)
return child;
if (recursively) {
Thing foundChild = child.GetChild(thingId, recursively);
if (foundChild != null)
return foundChild;
}
}
return null;
}
/// <summary>
/// Find a child by name
/// </summary>
/// <param name="name">The name of the child thing</param>
/// <param name="recursively">If true, the name will be searched through descendants recursively</param>
/// <returns>The found thing or null when nothing is found</returns>
Thing FindChild(string name, bool recursively = true) {
foreach (Thing child in this.children) {
if (child == null)
continue;
if (child.name == name)
return child;
if (recursively) {
Thing foundChild = child.FindChild(name, recursively);
if (foundChild != null)
return foundChild;
}
}
return null;
}
/// <summary>
/// Indicator that the hierarchy of the thing has changed
/// </summary>
public bool hierarchyChanged = true;
/// <summary>
/// The children of this thing
/// </summary>
[NonSerialized]
protected List<Thing> children = new();
private string _name = "";
/// <summary>
/// The name of the thing
/// </summary>
public virtual string name {
get => _name;
set {
if (_name != value) {
_name = value;
nameChanged = true;
OnNameChanged?.Invoke();
}
}
}
/// <summary>
/// Event which is triggered when the name changes
/// </summary>
public event ChangeHandler OnNameChanged = delegate { };
public bool nameChanged = false;
/// <summary>
/// An URL pointing to the location where a model of the thing can be found
/// </summary>
public string modelUrl = "";
private Spherical _position = Spherical.zero;
/// <summary>
/// The position of the thing in local space, in meters.
/// </summary>
public Spherical position {
get { return _position; }
set {
if (_position != value) {
_position = value;
positionUpdated = true;
//OnPositionChanged?.Invoke();
}
}
}
/// <summary>
/// Event triggered when the pose has changed
/// </summary>
public event ChangeHandler OnPoseChanged = null; //delegate { };
/// <summary>
/// Boolean indicating that the thing has an updated position
/// </summary>
public bool positionUpdated = false;
private SwingTwist _orientation = SwingTwist.zero;
/// <summary>
/// The orientation of the thing in local space
/// </summary>
public SwingTwist orientation {
get { return _orientation; }
set {
if (_orientation != value) {
_orientation = value;
orientationUpdated = true;
//OnOrientationChanged?.Invoke();
}
}
}
/// <summary>
/// Boolean indicating the thing has an updated orientation
/// </summary>
public bool orientationUpdated = false;
private Spherical _linearVelocity = Spherical.zero;
/// <summary>
/// The linear velocity of the thing in local space in meters per second
/// </summary>
public Spherical linearVelocity {
get => _linearVelocity;
set {
if (_linearVelocity != value) {
_linearVelocity = value;
linearVelocityUpdated = true;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
}
/// <summary>
/// Event triggered when the linear velocity has changed
/// </summary>
public event SphericalHandler OnLinearVelocityChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated linear velocity
/// </summary>
public bool linearVelocityUpdated = false;
private Spherical _angularVelocity = Spherical.zero;
/// <summary>
/// The angular velocity of the thing in local space in degrees per second
/// </summary>
public Spherical angularVelocity {
get => _angularVelocity;
set {
if (_angularVelocity != value) {
_angularVelocity = value;
angularVelocityUpdated = true;
OnAngularVelocityChanged?.Invoke(_angularVelocity);
}
}
}
/// <summary>
/// Event triggered when the angular velocity has changed
/// </summary>
public event SphericalHandler OnAngularVelocityChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated angular velocity
/// </summary>
public bool angularVelocityUpdated = false;
#if UNITY_5_3_OR_NEWER
/// <summary>
/// A reference to the representation of the thing in Unity
/// </summary>
[NonSerialized]
public Unity.Thing component = null;
#endif
public bool terminate = false;
#endregion Properties
#region Methods
public static ulong GetTimeMs() {
#if UNITY_5_3_OR_NEWER
return (ulong)(UnityEngine.Time.time * 1000);
#endif
}
/// <summary>
/// Update de state of the thing
/// </summary>
/// <param name="recursively">When true, this will Update the descendants recursively</param>
public void Update(bool recursively = false) {
Update(TimeManager.GetCurrentTimeMilliseconds(), recursively);
}
// #endif
/// <summary>
/// Update this thing
/// </summary>
/// <param name="currentTime">The current time in milliseconds</param>
public virtual void Update(ulong currentTimeMs, bool recursively = false) {
if (this.positionUpdated || this.orientationUpdated)
OnPoseChanged?.Invoke();
this.positionUpdated = false;
this.orientationUpdated = false;
this.linearVelocityUpdated = false;
this.angularVelocityUpdated = false;
//this.hierarchyChanged = false;
// should recurse over children...
if (recursively) {
for (byte childIx = 0; childIx < this.children.Count; childIx++) {
Thing child = this.children[childIx];
if (child == null)
continue;
child.Update(currentTimeMs, recursively);
}
}
}
/// <summary>
/// Function used to generate binary data for this thing
/// </summary>
/// <returns>a byte array with the binary data</returns>
/// @sa Passer::RoboidControl::BinaryMsg
public virtual byte[] GenerateBinary() { return Array.Empty<byte>(); }
/// <summary>
/// Function used to process binary data received for this thing
/// </summary>
/// <param name="bytes">The binary data</param>
public virtual void ProcessBinary(byte[] bytes) {
}
#endregion Methods
/// <summary>
/// Event triggered when a new thing has been created
/// </summary>
public static event ThingHandler OnNewThing = delegate { };
/// <summary>
/// Trigger the creation for the given thing
/// </summary>
/// <param name="thing">The created thing</param>
public static void InvokeNewThing(Thing thing) {
OnNewThing?.Invoke(thing);
}
/// <summary>
/// Check if the thing has the given properaties
/// </summary>
/// <param name="thing">The thing to check</param>
/// <param name="networkId">The network ID to compare to</param>
/// <param name="thingId">The thing ID to compare to</param>
/// <returns>True when the thing has the given properties</returns>
public static bool IsThing(Thing thing, byte networkId, byte thingId) {
if (thing == null)
return false;
return (thing.networkId == networkId) && (thing.id == thingId);
//return (thing.id == thingId);
}
}
}

50
src/Things/DifferentialDrive.cs Normal file
View File

@ -0,0 +1,50 @@
namespace RoboidControl {
/// @brief A thing which can move itself using a differential drive system
///
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
public class DifferentialDrive : Thing {
/// @brief Create a differential drive without networking support
public DifferentialDrive() { }
/// @brief Create a differential drive with networking support
/// @param participant The local participant
public DifferentialDrive(ParticipantUDP participant) : base(participant, Type.Undetermined) { }
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters
/// @param wheelSeparation The distance between the wheels in meters
///
/// These values are used to compute the desired wheel speed from the set
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) { }
/// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel
public void SetMotors(Thing leftWheel, Thing rightWheel) { }
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
/// Positive moves the robot in the forward direction.
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
public void SetWheelVelocity(float speedLeft, float speedRight) { }
/// @copydoc RoboidControl::Thing::Update(unsigned long)
public override void Update(ulong currentMs, bool recursive = true) { }
/// @brief The radius of a wheel in meters
protected float wheelRadius = 1.0f;
/// @brief The distance between the wheels in meters
protected float wheelSeparation = 1.0f;
/// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f;
/// @brief The left wheel
protected Thing leftWheel = null;
/// @brief The right wheel
protected Thing rightWheel = null;
};
} // namespace RoboidControl

44
src/Things/DistanceSensor.cs Normal file
View File

@ -0,0 +1,44 @@
namespace RoboidControl {
/// <summary>
/// A sensor measuring the distance in the forward direction
/// </summary>
public class DistanceSensor : Thing {
/// <summary>
/// The current measured distance
/// </summary>
public float distance = 0;
/// <summary>
/// Constructor for a new distance sensor
/// </summary>
/// <param name="participant">The participant for which the sensor is needed</param>
public DistanceSensor(Participant participant) : base(participant, Type.Undetermined) { }
/// <summary>
/// Create a distance sensor with the given ID
/// </summary>
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
public DistanceSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) {
}
#if UNITY_5_3_OR_NEWER
/// @copydoc Passer::RoboidControl::Thing::CreateComponent
public override void CreateComponent() {
this.component = Unity.DistanceSensor.Create(this);
this.component.core = this;
}
#endif
/// <summary>
/// Function to extract the distance received in the binary message
/// </summary>
/// <param name="bytes">The byte array</param>
public override void ProcessBinary(byte[] bytes) {
byte ix = 0;
this.distance = LowLevelMessages.ReceiveFloat16(bytes, ref ix);
}
}
}

33
src/Things/TemperatureSensor.cs Normal file
View File

@ -0,0 +1,33 @@
//using System;
namespace RoboidControl {
/// <summary>
/// A temperature sensor
/// </summary>
public class TemperatureSensor : Thing {
/// <summary>
/// The measured temperature
/// </summary>
public float temperature = 0;
/// <summary>
/// Create a temperature sensor with the given ID
/// </summary>
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
/// <summary>
/// Function to extract the temperature received in the binary message
/// </summary>
/// <param name="bytes">The byte array</param>
public override void ProcessBinary(byte[] bytes) {
byte ix = 0;
this.temperature = LowLevelMessages.ReceiveFloat16(bytes, ref ix);
//Console.WriteLine($"temperature {this.name} = {this.temperature} C");
}
}
}

65
src/Things/TouchSensor.cs Normal file
View File

@ -0,0 +1,65 @@
using System;
namespace RoboidControl {
/// <summary>
/// A sensor which can detect touches
/// </summary>
public class TouchSensor : Thing {
/// <summary>
/// Create a touch sensor
/// </summary>
/// <param name="owner">The participant for with the sensor is needed</param>
/// <param name="invokeEvent">True when the creation should trigger an event</param>
public TouchSensor(Participant owner) : base(owner, Type.TouchSensor) {
Console.Write("TouchSensor constructor");
//touchedSomething = false;
//thisParticipant = owner;
}
public TouchSensor(Participant owner, byte networkId, byte thingId) : base(owner, networkId, thingId) {
// Console.Write("TouchSensor constructor");
//touchedSomething = false;
//thisParticipant = participant;
}
public TouchSensor(Thing parent, bool invokeEvent = true) : base(parent, (byte)Type.TouchSensor, invokeEvent) { }
public ParticipantUDP thisParticipant;
/// <summary>
/// Value which is true when the sensor is touching something, false otherwise
/// </summary>
//public bool touchedSomething = false;
private bool _touchedSomething = false;
public bool touchedSomething {
get { return _touchedSomething; }
set {
if (_touchedSomething != value) {
touchUpdated = true;
_touchedSomething = value;
}
}
}
private bool touchUpdated = false;
#if UNITY_5_3_OR_NEWER
/// @copydoc Passer::RoboidControl::Thing::CreateComponent
public override void CreateComponent() {
// System.Console.Write("Create touch sensor component");
this.component = Unity.TouchSensor.Create(this);
this.component.core = this;
}
#endif
public override byte[] GenerateBinary() {
if (!touchUpdated)
return new byte[0];
byte[] buffer = new byte[1];
buffer[0] = (byte)(touchedSomething ? 1 : 0);
touchUpdated = false;
return buffer;
}
}
}

25
src/TimeManager.cs Normal file
View File

@ -0,0 +1,25 @@
using System.Diagnostics;
namespace RoboidControl {
/// <summary>
/// Time manager is een tool mainly to get the current running time in milliseconds
/// </summary>
public static class TimeManager {
private static readonly Stopwatch _stopwatch = new Stopwatch();
/// <summary>
/// Static constructor to start the stopwatch
/// </summary>
static TimeManager() {
_stopwatch.Start();
}
/// <summary>
/// Method to get the current time in milliseconds
/// </summary>
/// <returns>The current time in milliseconds</returns>
public static ulong GetCurrentTimeMilliseconds() {
return (ulong)_stopwatch.ElapsedMilliseconds;
}
}
}

19
test/RoboidControl_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\RoboidControl.csproj" />
</ItemGroup>
</Project>

87
test/UnitTest1.cs Normal file
View File

@ -0,0 +1,87 @@
#if !UNITY_5_3_OR_NEWER
using System;
using System.Threading;
using NUnit.Framework;
using RoboidControl;
namespace RoboidControl.test {
public class Tests {
[SetUp]
public void Setup() {
}
[Test]
public void Test_Participant() {
ParticipantUDP participant = new ParticipantUDP("127.0.0.1", 7682);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant.Update(milliseconds);
Thread.Sleep(100);
milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
}
Assert.Pass();
}
[Test]
public void Test_SiteServer() {
SiteServer siteServer = new SiteServer(7681);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer.Update(milliseconds);
Thread.Sleep(100);
milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
}
Assert.Pass();
}
[Test]
public void Test_SiteParticipant() {
SiteServer siteServer = new SiteServer(7681);
ParticipantUDP participant = new ParticipantUDP("127.0.0.1", 7681);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
while (milliseconds < startTime + 1000) {
siteServer.Update(milliseconds);
participant.Update(milliseconds);
Thread.Sleep(100);
milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
}
Assert.That(participant.networkId, Is.EqualTo(1));
}
[Test]
public void Test_ThingMsg() {
SiteServer siteServer = new SiteServer(7681);
ParticipantUDP participant = new ParticipantUDP("127.0.0.1");
Thing thing = new Thing(participant) {
name = "First Thing",
modelUrl = "https://passer.life/extras/ant.jpg"
};
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer.Update(milliseconds);
participant.Update(milliseconds);
Thread.Sleep(100);
milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
}
Assert.That(participant.networkId, Is.EqualTo(1));
}
}
}
#endif