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

Restructuring

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-07 14:48:03 +01:00
parent 6a443e111c
commit 9ad1ec24d0
59 changed files with 858 additions and 5700 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
.editorconfig
View File

@ -1,19 +0,0 @@
# 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
View File

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

26
.gitlab-ci.yml
View File

@ -1,26 +0,0 @@
# 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
View File

@ -1,14 +0,0 @@
{
// 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"
}
]
}

8
DoxyGen.meta
View File

@ -1,8 +0,0 @@
fileFormatVersion: 2
guid: 1989654e8505b074d9a0280de8649b7d
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

2861
DoxyGen/Doxyfile
View File

File diff suppressed because it is too large Load Diff

7
DoxyGen/Doxyfile.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: db5f4144ac032d649994939f1d833737
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

226
DoxyGen/DoxygenLayout.xml
View File

@ -1,226 +0,0 @@
<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>

7
DoxyGen/DoxygenLayout.xml.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: dd31613f75db97f4ca4d408bfce69746
TextScriptImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

12
DoxyGen/custom_doxygen.css
View File

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

7
DoxyGen/custom_doxygen.css.meta
View File

@ -1,7 +0,0 @@
fileFormatVersion: 2
guid: 186de70a0b3d098409ce1a5ec887b1ae
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

11
Examples/BB2B/BB2B.csproj
View File

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

35
Examples/BB2B/Program.cs
View File

@ -1,35 +0,0 @@
using System;
using System.Diagnostics;
using System.Threading;
using RoboidControl;
class BB2B {
static void Main() {
// The robot's propulsion is a differential drive
DifferentialDrive bb2b = new DifferentialDrive();
// Is has a touch sensor at the front left of the roboid
TouchSensor touchLeft = new TouchSensor(bb2b);
// and other one on the right
TouchSensor touchRight = new TouchSensor(bb2b);
// Do forever:
while (true) {
Console.Write("A");
// 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);
}
}
}

10
README.md
View File

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

39
RoboidControl.sln
View File

@ -1,39 +0,0 @@

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}") = "test", "test\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
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
Release|Any CPU = Release|Any CPU
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
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
EndGlobalSection
GlobalSection(NestedProjects) = preSolution
{488F28B2-A2CC-4E32-8D3B-7DB4EB1485F9} = {D6086F71-404B-4D18-BBE9-45947ED33DB2}
EndGlobalSection
EndGlobal

25
Unity/DebugConsole.cs
View File

@ -1,25 +0,0 @@
#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
View File

@ -1,67 +0,0 @@
#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

38
Unity/SiteServer.cs
View File

@ -1,38 +0,0 @@
#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() {
site.Close();
}
public void HandleNewThing(RoboidControl.Thing thing) {
//Debug.Log("Handle New thing event");
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

115
Unity/Thing.cs
View File

@ -1,115 +0,0 @@
#if UNITY_5_3_OR_NEWER
using System.Collections;
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; }
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) {
Debug.LogWarning("No site server found");
return;
}
siteServer.site.Add(thing);
}
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.core = core;
if (core.parent != null && core.parent.component != null)
gameObj.transform.SetParent(core.parent.component.transform, false);
if (core.position != null)
gameObj.transform.localPosition = core.position.ToVector3();
return component;
}
/// <summary>
/// Update the Unity representation
/// </summary>
protected virtual void Update() {
if (core == null)
return;
if (core.linearVelocity != null) {
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) {
Vector3 angularVelocity = core.angularVelocity.ToVector3();
this.transform.localRotation *= Quaternion.Euler(angularVelocity * Time.deltaTime);
}
if (core.hasPosition)
this.transform.localPosition = core.position.ToVector3();
//this.transform.localRotation = core.orientation.ToQuaternion();
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;
}
}
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

87
Unity/TouchSensor.cs
View File

@ -1,87 +0,0 @@
#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));
}
// Somehow this does not work.
// Rigidbody rb = GetComponentInParent<Rigidbody>();
// if (rb == null) {
// RoboidControl.Thing thing = core;
// while (thing.parent != null)
// thing = thing.parent;
// Thing unityThing = thing.component;
// rb = unityThing.gameObject.AddComponent<Rigidbody>();
// rb.isKinematic = true;
// }
}
/// <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>();
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.01F;
collider.isTrigger = true;
component.core = core;
component.participant = FindAnyObjectByType<SiteServer>();
core.thisParticipant = component.participant.site;
if (core.parent != null && core.parent.component != null)
gameObj.transform.SetParent(core.parent.component.transform, false);
if (core.position != null)
gameObj.transform.localPosition = core.position.ToVector3();
return component;
}
private void OnTriggerEnter(Collider other) {
if (other.isTrigger)
return;
if (this.transform.root == other.transform.root)
return;
this.coreSensor.touchedSomething = true;
}
private void OnTriggerExit(Collider other) {
if (other.isTrigger)
return;
this.coreSensor.touchedSomething = false;
}
}
}
#endif

110
src/Angle.cs Normal file
View File

@ -0,0 +1,110 @@
using System;
namespace Passer.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 Deg2Rad = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
public const float Rad2Deg = ((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);
//}
}
}

0
src/LinearAlgebra/Direction.cs → src/Direction.cs
View File

41
src/Float.cs Normal file
View File

@ -0,0 +1,41 @@
namespace Passer.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);
}
}
}

2
src/RoboidControl.csproj → src/LinearAlgebra.csproj
View File

@ -1,4 +1,4 @@
<Project Sdk="Microsoft.NET.Sdk"> <Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup> <PropertyGroup>
<GenerateAssemblyInfo>false</GenerateAssemblyInfo> <GenerateAssemblyInfo>false</GenerateAssemblyInfo>

11
src/LinearAlgebra/Angle.cs
View File

@ -1,11 +0,0 @@
using System;
namespace Passer.LinearAlgebra {
public 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;
}
}

51
src/LinearAlgebra/Vector2.cs
View File

@ -1,51 +0,0 @@
using System;
using System.Numerics;
namespace Passer.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;
}
}
}

33
src/LinearAlgebra/Vector3.cs
View File

@ -1,33 +0,0 @@
#if !UNITY_5_3_OR_NEWER
using System;
namespace Passer.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);
}
}
}
#endif

353
src/LocalParticipant.cs
View File

@ -1,353 +0,0 @@
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 LocalParticipant : Participant {
public byte[] buffer = new byte[1024];
public ulong publishInterval = 3000; // = 3 seconds
public string name = "Participant";
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 porticiapnt
/// </summary>
public LocalParticipant() {
//senders.Add(this);
}
/// <summary>
/// Create a participant with the give UDP port
/// </summary>
/// <param name="port">The port number on which to communicate</param>
public LocalParticipant(int port) : this() {
this.port = port;
}
/// <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 LocalParticipant(string ipAddress = "0.0.0.0", int port = 7681) : this() {
this.ipAddress = ipAddress;
this.port = port;
this.udpClient = new UdpClient();
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); // local port
// this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
// this.udpClient = new UdpClient(port); // for receiving
// this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
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 LocalParticipant(UdpClient udpClient, int port) : this() {
this.udpClient = udpClient;
this.port = port;
}
private static LocalParticipant isolatedParticipant = null;
public static LocalParticipant Isolated() {
if (isolatedParticipant == null)
isolatedParticipant = new LocalParticipant(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();
public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
public void Register(byte thingType, ThingConstructor constr) {
thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
}
public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
Register<ThingClass>((byte)thingType);
}
public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
}
#endregion Init
#region Update
protected void ReceiveUDP(IAsyncResult result) {
if (this.udpClient == null || this.endPoint == null)
return;
byte[] data = this.udpClient.EndReceive(result, ref this.endPoint);
// This does not yet take multi-packet messages into account!
if (this.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 = this.endPoint.Address.ToString();
Participant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, this.endPoint.Port);
ReceiveData(data, remoteParticipant);
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
protected ulong nextPublishMe = 0;
public virtual void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0) {
#if UNITY_5_3_OR_NEWER
currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
#endif
}
if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) {
Publish();
// Console.WriteLine($"{this.name} Publish ClientMsg {this.networkId}");
this.nextPublishMe = currentTimeMS + this.publishInterval;
}
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing != null) {
thing.Update(currentTimeMS);
// if (thing.owner != this) {
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(thing.owner, binaryMsg);
// }
}
}
}
public virtual void Publish() {
this.Publish(new ParticipantMsg(this.networkId));
}
#endregion Update
#region Send
public void SendThingInfo(Participant remoteParticipant, Thing thing) {
Console.WriteLine("Send thing info");
this.Send(remoteParticipant, new ThingMsg(this.networkId, thing));
this.Send(remoteParticipant, new NameMsg(this.networkId, thing));
this.Send(remoteParticipant, new ModelUrlMsg(this.networkId, thing));
this.Send(remoteParticipant, new BinaryMsg(this.networkId, thing));
}
public bool Send(Participant remoteParticipant, IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.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
public void ReceiveData(byte[] data, Participant remoteParticipant) {
byte msgId = data[0];
if (msgId == 0xFF) {
// Timeout
return;
}
switch (msgId) {
case ParticipantMsg.Id: // 0xA0 / 160
this.Process(remoteParticipant, new ParticipantMsg(data));
break;
case NetworkIdMsg.Id: // 0xA1 / 161
this.Process(remoteParticipant, new NetworkIdMsg(data));
break;
case InvestigateMsg.Id: // 0x81
// result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.id: // 0x80 / 128
this.Process(remoteParticipant, new ThingMsg(data));
break;
case NameMsg.Id: // 0x91 / 145
this.Process(remoteParticipant, new NameMsg(data));
break;
case ModelUrlMsg.Id: // 0x90 / 144
this.Process(remoteParticipant, new ModelUrlMsg(data));
break;
case PoseMsg.Id: // 0x10 / 16
this.Process(remoteParticipant, new PoseMsg(data));
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case BinaryMsg.Id: // 0xB1 / 177
this.Process(remoteParticipant, new BinaryMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
break;
case DestroyMsg.Id: // 0x20 / 32
// result = await DestroyMsg.Receive(dataStream, client, packetSize);
break;
default:
break;
}
}
#endregion
#region Process
protected virtual void Process(Participant sender, ParticipantMsg msg) { }
protected virtual void Process(Participant sender, NetworkIdMsg msg) {
Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}");
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) { }
protected virtual void Process(Participant sender, ThingMsg msg) {
//Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
}
protected virtual void Process(Participant sender, NameMsg msg) {
//Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.name = msg.name;
}
protected virtual void Process(Participant sender, ModelUrlMsg msg) {
//Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.modelUrl = msg.url;
}
protected virtual void Process(Participant sender, PoseMsg msg) {
//Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) {
thing.hasPosition = false;
if ((msg.poseType & PoseMsg.Pose_Position) != 0) {
thing.position = msg.position;
thing.hasPosition = true;
}
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
thing.orientation = msg.orientation;
else
thing.orientation = null;
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
thing.angularVelocity = msg.angularVelocity;
}
}
protected virtual void Process(Participant sender, BinaryMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.bytes);
}
protected virtual void Process(Participant sender, TextMsg temsgxt) { }
protected virtual void Process(Participant sender, DestroyMsg msg) { }
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
}
}

0
src/LinearAlgebra/Matrix.cs → src/Matrix.cs
View File

83
src/Messages/BinaryMsg.cs
View File

@ -1,83 +0,0 @@
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 = 2;
/// <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 binary data
/// </summary>
public byte[] bytes;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="bytes">The binary data for the thing</param>
public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
/// <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.bytes = this.thing.GenerateBinary();
// if (this.bytes.Length > 0)
// System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
return 0;
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;
}
}
}

51
src/Messages/DestroyMsg.cs
View File

@ -1,51 +0,0 @@
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, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public DestroyMsg(byte[] buffer) : base(buffer) { }
/// @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
View File

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

112
src/Messages/LowLevelMessages.cs
View File

@ -1,112 +0,0 @@
using Passer.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 = new Spherical(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
View File

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

77
src/Messages/ModelUrlMsg.cs
View File

@ -1,77 +0,0 @@
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 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.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.url = url;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public ModelUrlMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
int strlen = buffer[ix++];
url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (this.url == null)
return 0;
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;
}
}
}

82
src/Messages/NameMsg.cs
View File

@ -1,82 +0,0 @@
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 len;
/// <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;
}
/// <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++];
int strlen = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
return 0;
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
int nameLength = this.name.Length;
buffer[ix++] = (byte)nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;
}
}
}

43
src/Messages/NetworkIdMsg.cs
View File

@ -1,43 +0,0 @@
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;
buffer[0] = NetworkIdMsg.Id;
buffer[1] = this.networkId;
return NetworkIdMsg.length;
}
}
}

53
src/Messages/ParticipantMsg.cs
View File

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

133
src/Messages/PoseMsg.cs
View File

@ -1,133 +0,0 @@
using Passer.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;
}
/// @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) {
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;
}
}
}

45
src/Messages/TextMsg.cs
View File

@ -1,45 +0,0 @@
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 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.text = text;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public TextMsg(byte[] buffer) : base(buffer) { }
/// @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;
byte ix = 0;
buffer[ix++] = TextMsg.Id;
buffer[ix++] = (byte)this.text.Length;
for (int textIx = 0; textIx < this.text.Length; textIx++)
buffer[ix++] = (byte)this.text[textIx];
return ix;
}
}
}

90
src/Messages/ThingMsg.cs
View File

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

84
src/Participant.cs
View File

@ -1,84 +0,0 @@
using System;
using System.Collections.Generic;
namespace RoboidControl {
/// <summary>
/// A reference to a participant, possibly on a remote location
/// </summary>
public class Participant {
/// <summary>
/// The internet address of the participant
/// </summary>
public string ipAddress = "0.0.0.0";
/// <summary>
/// The UDP port on which the participant can be reached
/// </summary>
public int port = 0;
/// <summary>
/// The network ID of the participant
/// </summary>
public byte networkId;
/// <summary>
/// Default constructor
/// </summary>
public Participant() { }
/// <summary>
/// Create a new remote participant
/// </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 things reported by this participant
/// </summary>
protected readonly List<Thing> things = new List<Thing>();
/// <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 => 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 invokeEvent = true) {
// Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]");
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
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}]");
}
}
}
}
}

0
src/LinearAlgebra/Quat32.cs → src/Quat32.cs
View File

0
src/LinearAlgebra/Quaternion.cs → src/Quaternion.cs
View File

81
src/SiteServer.cs
View File

@ -1,81 +0,0 @@
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 : LocalParticipant {
public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
/// <summary>
/// Create a new site server
/// </summary>
/// <param name="port"></param>
public SiteServer(string ipAddress = "0.0.0.0", int port = 7681) : base() {
this.name = "Site Server";
this.ipAddress = ipAddress;
this.port = port;
this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
Console.Write($"Prepare receive on port {port}");
this.udpClient = new UdpClient(port); // for receiving
this.udpClient.BeginReceive(
new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
Register<TouchSensor>(Thing.Type.TouchSensor);
}
/// <summary>
/// Close the site
/// </summary>
public void Close() {
this.udpClient?.Close();
}
public override void Publish() {
}
protected override void Process(Participant remoteParticipant, ParticipantMsg msg) {
if (msg.networkId == 0) {
Console.WriteLine($"{this.name} received New Client -> {remoteParticipant.networkId}");
this.Send(remoteParticipant, new NetworkIdMsg(remoteParticipant.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}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) {
Thing newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
//Console.WriteLine("Found thing message processor");
if (msgProcessor != null)
newThing = msgProcessor(sender, msg.networkId, msg.thingId);
}
if (newThing == null) {
newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
// Console.WriteLine("Created generic new core thing");
}
if (msg.parentId != 0) {
Thing parentThing = Get(msg.networkId, msg.parentId);
if (parentThing == null)
Console.WriteLine("Could not find parent");
else
newThing.parent = parentThing;
}
Console.WriteLine("Adding to remote sender");
sender.Add(newThing);
}
}
}
}

0
src/LinearAlgebra/Spherical.cs → src/Spherical.cs
View File

0
src/LinearAlgebra/SwingTwist.cs → src/SwingTwist.cs
View File

327
src/Thing.cs
View File

@ -1,327 +0,0 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using Passer.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
};
#endregion Types
#region Init
public Thing(byte thingType = (byte)Type.Undetermined) : this(LocalParticipant.Isolated(), thingType) {
}
public Thing(Participant owner, byte thingType = (byte)Type.Undetermined) {
this.owner = owner;
this.type = thingType;
}
public Thing(Thing parent, byte thingType = (byte)Type.Undetermined) : this(parent.owner, thingType) {
this.parent = parent;
}
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="owner">The participant for which this thing is created</param>
/// <param name="invokeEvent">True when a new thing event should be triggered</param>
public Thing(Participant owner, bool invokeEvent = false) {
this.owner = owner;
//owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
}
/// <summary>
/// Create a new thing for the given participant
/// </summary>
/// <param name="participant">The participant for which this thing is created</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 participant, byte networkId, byte thingId, byte thingType = 0) {
this.owner = participant;
this.id = thingId;
this.type = thingType;
this.networkId = networkId;
}
/// <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
public delegate void ChangeHandler();
public delegate void SphericalHandler(Spherical v);
public delegate void ThingHandler(Thing t);
/// <summary>
/// The participant to which this thing belongs
/// </summary>
public Participant owner;
/// <summary>
/// The network ID of this thing.
/// </summary>
public byte networkId;
/// <summary>
/// The ID of this thing
/// </summary>
public byte id;
/// <summary>
/// The type of this thing. This can be either a Thing::Type (needs casting)
/// or a byte value for custom types.
/// </summary>
public byte type;
/// <summary>
/// Event which is triggered when the parent changes
/// </summary>
public event ChangeHandler OnParentChanged = delegate { };
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);
OnParentChanged?.Invoke();
}
}
}
/// <summary>
/// Attach a thing as a child of this thing
/// </summary>
/// <param name="child">The thing to attach as a child</param>
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>
public void RemoveChild(Thing child) {
children.Remove(child);
}
/// <summary>
/// The list of children of this thing
/// </summary>
[NonSerialized]
public List<Thing> children = new List<Thing>();
/// <summary>
/// Event which is triggered when the name changes
/// </summary>
public event ChangeHandler OnNameChanged = delegate { };
private string _name = "";
/// <summary>
/// The name of the thing
/// </summary>
public virtual string name {
get => _name;
set {
if (_name != value) {
_name = value;
OnNameChanged?.Invoke();
}
}
}
/// <summary>
/// An URL pointing to the location where a model of the thing can be found
/// </summary>
public string modelUrl = "";
/// <summary>
/// Event triggered when the position has changed
/// </summary>
public event ChangeHandler OnPositionChanged = delegate { };
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;
OnPositionChanged?.Invoke();
}
}
}
public bool hasPosition = false;
/// <summary>
/// Event triggered when the orientation has changed
/// </summary>
public event ChangeHandler OnOrientationChanged = delegate { };
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;
OnOrientationChanged?.Invoke();
}
}
}
/// <summary>
/// Event triggered when the linear velocity has changed
/// </summary>
public event SphericalHandler OnLinearVelocityChanged = delegate { };
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;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
}
/// <summary>
/// The angular velocity of the thing in local space
/// </summary>
public Spherical angularVelocity = Spherical.zero;
#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
#endregion Properties
#region Update
// #if UNITY_5_3_OR_NEWER
/// <summary>
/// Convience function for use in Unity which removes the need for a currentTime argument
/// </summary>
public void Update(bool recursive = false) {
Update(TimeManager.GetCurrentTimeMilliseconds(), recursive);
}
// #endif
/// <summary>
/// Update this thing
/// </summary>
/// <param name="currentTime">The current time in milliseconds</param>
public virtual void Update(ulong currentTimeMs, bool recursive = false) {
// should recurse over children...
if (recursive) {
for (byte childIx = 0; childIx < this.children.Count; childIx++) {
Thing child = this.children[childIx];
if (child == null)
continue;
child.Update(currentTimeMs, recursive);
}
}
}
/// <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 new byte[0]; }
/// <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 Update
/// <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);
}
}
}

51
src/Things/DifferentialDrive.cs
View File

@ -1,51 +0,0 @@
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(LocalParticipant participant) : base(participant, false) { }
/// @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
View File

@ -1,44 +0,0 @@
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, true) { }
/// <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
View File

@ -1,33 +0,0 @@
//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(LocalParticipant 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");
}
}
}

61
src/Things/TouchSensor.cs
View File

@ -1,61 +0,0 @@
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, bool invokeEvent = true) : base(owner, invokeEvent) {
//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) : base(parent) { }
public LocalParticipant 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 {
_touchedSomething = value;
if (thisParticipant != null && this.owner != thisParticipant) {
BinaryMsg msg = new(networkId, this);
foreach (Participant remoteParticipant in thisParticipant.owners)
thisParticipant.Send(remoteParticipant, msg);
}
}
}
#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() {
byte[] buffer = new byte[1];
buffer[0] = (byte)(touchedSomething ? 1 : 0);
return buffer;
}
}
}

22
src/TimeManger.cs
View File

@ -1,22 +0,0 @@
using System.Diagnostics;
namespace RoboidControl {
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;
}
}
}

363
src/Vector2.cs Normal file
View File

@ -0,0 +1,363 @@
using System;
using System.Numerics;
namespace Passer.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
src/Vector3.cs Normal file
View File

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

0
src/LinearAlgebra/float16.cs → src/float16.cs
View File

162
test/AngleTest.cs Normal file
View File

@ -0,0 +1,162 @@
#if NUNIT
using NUnit.Framework;
using Passer.LinearAlgebra;
namespace LinearAlgebraTest {
public class AngleTest {
[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

4
test/test.csproj → test/LinearAlgebra_Test.csproj
View File

@ -1,4 +1,4 @@
<Project Sdk="Microsoft.NET.Sdk"> <Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup> <PropertyGroup>
<TargetFramework>net5.0</TargetFramework> <TargetFramework>net5.0</TargetFramework>
@ -13,7 +13,7 @@
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ProjectReference Include="..\src\RoboidControl.csproj" /> <ProjectReference Include="..\src\LinearAlgebra.csproj" />
</ItemGroup> </ItemGroup>
</Project> </Project>

87
test/UnitTest1.cs
View File

@ -1,87 +0,0 @@
#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() {
LocalParticipant participant = new LocalParticipant("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);
LocalParticipant participant = new LocalParticipant("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);
LocalParticipant participant = new LocalParticipant("127.0.0.1");
Thing thing = new Thing(participant, false) {
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