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

Compare commits

base: RoboidControl:8357c5d6231947a3ecd32a81b723506640fff441
Branches Tags
RoboidControl:main
..
compare: RoboidControl:d82388fc453f05d70638684174078c2533c238f2
Branches Tags
RoboidControl:main

No commits in common. "8357c5d6231947a3ecd32a81b723506640fff441" and "d82388fc453f05d70638684174078c2533c238f2" have entirely different histories.
8357c5d623 ... d82388fc45

16 changed files with 130 additions and 514 deletions
Show Stats Expand all files Collapse all files

287
LinearAlgebra/src/Decomposition.cs
View File

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

1
Unity/SiteServer.cs
View File

@ -18,7 +18,6 @@ namespace RoboidControl.Unity {
}
void OnApplicationQuit() {
if (site != null)
site.Close();
}

4
Unity/Thing.cs
View File

@ -31,7 +31,7 @@ namespace RoboidControl.Unity {
core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null || siteServer.site == null) {
if (siteServer == null) {
Debug.LogWarning("No site server found");
return;
}
@ -93,7 +93,7 @@ namespace RoboidControl.Unity {
}
private void PoseChanged() {
// Debug.Log($"{this} pose changed");
Debug.Log($"{this} pose changed");
if (core.positionUpdated)
this.transform.localPosition = core.position.ToVector3();
if (core.orientationUpdated)

139
src/ParticipantUDP.cs → src/LocalParticipant.cs
View File

@ -9,7 +9,7 @@ namespace RoboidControl {
/// <summary>
/// A participant is used for communcation between things
/// </summary>
public class ParticipantUDP : Participant {
public class LocalParticipant : Participant {
public byte[] buffer = new byte[1024];
public ulong publishInterval = 3000; // = 3 seconds
@ -26,7 +26,7 @@ namespace RoboidControl {
/// <summary>
/// Create a porticiapnt
/// </summary>
public ParticipantUDP() {
public LocalParticipant() {
//senders.Add(this);
}
@ -34,7 +34,7 @@ namespace RoboidControl {
/// Create a participant with the give UDP port
/// </summary>
/// <param name="port">The port number on which to communicate</param>
public ParticipantUDP(int port) : this() {
public LocalParticipant(int port) : this() {
this.port = port;
}
@ -43,7 +43,7 @@ namespace RoboidControl {
/// </summary>
/// <param name="ipAddress">The ip address of the site server</param>
/// <param name="port">The port number of the site server</param>
public ParticipantUDP(string ipAddress = "0.0.0.0", int port = 7681) : this() {
public LocalParticipant(string ipAddress = "0.0.0.0", int port = 7681) : this() {
this.ipAddress = ipAddress;
this.port = port;
@ -61,15 +61,15 @@ namespace RoboidControl {
/// </summary>
/// <param name="udpClient">UDP client to use for communication</param>
/// <param name="port">The port number on which to communicate</param>
public ParticipantUDP(UdpClient udpClient, int port) : this() {
public LocalParticipant(UdpClient udpClient, int port) : this() {
this.udpClient = udpClient;
this.port = port;
}
private static ParticipantUDP isolatedParticipant = null;
public static ParticipantUDP Isolated() {
private static LocalParticipant isolatedParticipant = null;
public static LocalParticipant Isolated() {
if (isolatedParticipant == null)
isolatedParticipant = new ParticipantUDP(0);
isolatedParticipant = new LocalParticipant(0);
return isolatedParticipant;
}
@ -94,20 +94,20 @@ namespace RoboidControl {
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 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>(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}");
// }
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
@ -134,7 +134,7 @@ namespace RoboidControl {
}
protected ulong nextPublishMe = 0;
public override void Update(ulong currentTimeMS = 0) {
public virtual void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0) {
#if UNITY_5_3_OR_NEWER
currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
@ -152,20 +152,13 @@ namespace RoboidControl {
Thing thing = this.things[ix];
if (thing != null && thing.parent == null) {
thing.Update(currentTimeMS, true);
// if (this.isIsolated == false) {
if (thing.owner != this) { // should not happen....
PoseMsg poseMsg = new(thing.owner.networkId, thing);
this.Send(thing.owner, poseMsg);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
this.Send(thing.owner, binaryMsg);
// if (thing.owner != this) {
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(thing.owner, binaryMsg);
// }
}
}
}
for (int ownerIx = 0; ownerIx < this.owners.Count; ownerIx++) {
Participant owner = this.owners[ownerIx];
owner.Update(currentTimeMS);
}
}
public virtual void Publish() {
this.Publish(new ParticipantMsg(this.networkId));
@ -176,7 +169,7 @@ namespace RoboidControl {
#region Send
public void SendThingInfo(Participant owner, Thing thing) {
// Console.WriteLine("Send thing info");
Console.WriteLine("Send thing info");
this.Send(owner, new ThingMsg(this.networkId, thing));
this.Send(owner, new NameMsg(this.networkId, thing));
this.Send(owner, new ModelUrlMsg(this.networkId, thing));
@ -195,7 +188,7 @@ namespace RoboidControl {
}
public void PublishThingInfo(Thing thing) {
// Console.WriteLine("Publish thing info");
//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));
@ -207,7 +200,7 @@ namespace RoboidControl {
if (bufferSize <= 0)
return true;
Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
// Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
@ -234,7 +227,7 @@ namespace RoboidControl {
#region Receive
public void ReceiveData(byte[] data, Participant sender) {
public void ReceiveData(byte[] data, Participant remoteParticipant) {
byte msgId = data[0];
if (msgId == 0xFF) {
// Timeout
@ -243,29 +236,29 @@ namespace RoboidControl {
switch (msgId) {
case ParticipantMsg.Id: // 0xA0 / 160
this.Process(sender, new ParticipantMsg(data));
this.Process(remoteParticipant, new ParticipantMsg(data));
break;
case NetworkIdMsg.Id: // 0xA1 / 161
this.Process(sender, new NetworkIdMsg(data));
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(sender, new ThingMsg(data));
this.Process(remoteParticipant, new ThingMsg(data));
break;
case NameMsg.Id: // 0x91 / 145
this.Process(sender, new NameMsg(data));
this.Process(remoteParticipant, new NameMsg(data));
break;
case ModelUrlMsg.Id: // 0x90 / 144
this.Process(sender, new ModelUrlMsg(data));
this.Process(remoteParticipant, new ModelUrlMsg(data));
break;
case PoseMsg.Id: // 0x10 / 16
this.Process(sender, new PoseMsg(data));
this.Process(remoteParticipant, new PoseMsg(data));
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case BinaryMsg.Id: // 0xB1 / 177
this.Process(sender, new BinaryMsg(data));
this.Process(remoteParticipant, new BinaryMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
@ -282,17 +275,10 @@ namespace RoboidControl {
#region Process
protected virtual void Process(Participant sender, ParticipantMsg msg) {
#if DEBUG
Console.WriteLine($"{this.name} Process participantMsg {msg.networkId}");
#endif
}
protected virtual void Process(Participant sender, ParticipantMsg msg) { }
protected virtual void Process(Participant sender, NetworkIdMsg msg) {
#if DEBUG
Console.WriteLine($"{this.name} Process SiteMsg {this.networkId} -> {msg.networkId}");
#endif
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())
@ -300,76 +286,53 @@ namespace RoboidControl {
}
}
protected virtual void Process(Participant sender, InvestigateMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: InvestigateMsg [{msg.networkId}/{msg.thingId}]");
#endif
}
protected virtual void Process(Participant sender, InvestigateMsg msg) { }
protected virtual void Process(Participant sender, ThingMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId}");
#endif
//Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
}
protected virtual void Process(Participant sender, NameMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process NameMsg [{msg.networkId}/{msg.thingId}] {msg.nameLength} {msg.name}");
#endif
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) {
#if DEBUG
Console.WriteLine($"Participant: Process ModelUrlMsg [{msg.networkId}/{msg.thingId}] {msg.urlLength} {msg.url}");
#endif
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) {
#if DEBUG
Console.WriteLine($"Participant: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
#endif
//Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) {
if ((msg.poseType & PoseMsg.Pose_Position) != 0)
thing.position = msg.position;
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
thing.orientation = msg.orientation;
Console.Write($"{thing.id} orientation changed");
}
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) {
#if DEBUG
Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
#endif
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.data);
thing?.ProcessBinary(msg.bytes);
}
protected virtual void Process(Participant sender, TextMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process TextMsg {msg.textLength} {msg.text}");
#endif
protected virtual void Process(Participant sender, TextMsg temsgxt) { }
}
protected virtual void Process(Participant sender, DestroyMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}]");
#endif
}
protected virtual void Process(Participant sender, DestroyMsg msg) { }
private void ForwardMessage(IMessage msg) {
// foreach (Participant client in senders) {

41
src/Messages/BinaryMsg.cs
View File

@ -12,7 +12,7 @@ namespace RoboidControl {
/// The length of the message, excluding the binary data
/// </summary>
/// For the total size of the message this.bytes.Length should be added to this value.
public const byte length = 4;
public const byte length = 2;
/// <summary>
/// The network ID identifying the thing
/// </summary>
@ -23,15 +23,23 @@ namespace RoboidControl {
public byte thingId;
public Thing thing;
/// <summary>
/// The length of the data
/// </summary>
public byte dataLength;
/// <summary>
/// The binary data
/// </summary>
public byte[] data;
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>
@ -41,34 +49,31 @@ namespace RoboidControl {
this.networkId = networkId;
this.thingId = thing.id;
this.thing = thing;
this.data = this.thing.GenerateBinary();
this.dataLength = (byte)this.data.Length;
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++];
this.dataLength = buffer[ix++];
this.data = new byte[this.dataLength];
for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
this.data[dataIx] = 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.data.Length || this.data.Length == 0)
if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
#endif
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.dataLength;
foreach (byte b in data)
foreach (byte b in bytes)
buffer[ix++] = b;
return ix;

25
src/Messages/ModelUrlMsg.cs
View File

@ -21,10 +21,6 @@ namespace RoboidControl {
/// </summary>
public byte thingId;
/// <summary>
/// The length of the url string, excluding the null terminator
/// </summary>
public byte urlLength;
/// <summary>
/// The URL of the model
/// </summary>
public string url = null;
@ -37,7 +33,6 @@ namespace RoboidControl {
public ModelUrlMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.urlLength = (byte)thing.modelUrl.Length;
this.url = thing.modelUrl;
}
/// <summary>
@ -46,30 +41,26 @@ namespace RoboidControl {
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="url">The URL to send</param>
// public ModelUrlMsg(byte networkId, byte thingId, string url) {
// this.networkId = networkId;
// this.thingId = thingId;
// this.urlLength = (byte)url.Length;
// this.url = url;
// }
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++];
this.urlLength = buffer[ix++];
this.url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.urlLength);
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 (string.IsNullOrEmpty(this.url))
if (this.url == null)
return 0;
#if DEBUG
System.Console.WriteLine($"Send ModelUrlMsg [{this.networkId}/{this.thingId}] {this.urlLength} {this.url}");
#endif
byte ix = 0;
buffer[ix++] = ModelUrlMsg.Id;
buffer[ix++] = this.networkId;

30
src/Messages/NameMsg.cs
View File

@ -20,11 +20,10 @@ namespace RoboidControl {
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The length of the name, excluding null terminator
/// </summary>
public byte nameLength;
public byte len;
/// <summary>
/// The name of the thing, not terminated with a null character
/// </summary>
@ -39,7 +38,6 @@ namespace RoboidControl {
this.networkId = networkId;
this.thingId = thing.id;
this.name = thing.name;
this.nameLength = (byte)this.name.Length;
}
/// <summary>
/// Create a new message for sending
@ -47,34 +45,34 @@ namespace RoboidControl {
/// <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;
// }
public NameMsg(byte networkId, byte thingId, string name) {
this.networkId = networkId;
this.thingId = thingId;
this.name = name;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public NameMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
this.nameLength = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.nameLength);
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 || string.IsNullOrEmpty(this.name))
if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send NameMsg [{this.networkId}/{this.thingId}] {this.nameLength} {this.name}");
#endif
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.nameLength;
for (int nameIx = 0; nameIx < this.nameLength; nameIx++)
int nameLength = this.name.Length;
buffer[ix++] = (byte)nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;

3
src/Messages/NetworkIdMsg.cs
View File

@ -34,9 +34,6 @@ namespace RoboidControl {
if (buffer.Length < NetworkIdMsg.length)
return 0;
#if DEBUG
System.Console.WriteLine($"Send NetworkIdMsg {this.networkId}");
#endif
buffer[0] = NetworkIdMsg.Id;
buffer[1] = this.networkId;
return NetworkIdMsg.length;

28
src/Messages/PoseMsg.cs
View File

@ -88,34 +88,6 @@ namespace RoboidControl {
this.linearVelocity = linearVelocity;
this.angularVelocity = angularVelocity;
}
public PoseMsg(byte networkId, Thing thing, bool force = false) {
this.networkId = networkId;
this.thingId = thing.id;
this.poseType = 0;
if (thing.positionUpdated || force) {
this.position = thing.position;
this.poseType |= Pose_Position;
//thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here?
}
if (thing.orientationUpdated || force) {
this.orientation = thing.orientation;
this.poseType |= Pose_Orientation;
//thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here?
}
if (thing.linearVelocityUpdated) {
this.linearVelocity = thing.linearVelocity;
this.poseType |= Pose_LinearVelocity;
thing.linearVelocityUpdated = false;
}
if (thing.angularVelocityUpdated) {
this.angularVelocity = thing.angularVelocity;
this.poseType |= Pose_AngularVelocity;
thing.angularVelocityUpdated = false;
}
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public PoseMsg(byte[] buffer) : base(buffer) {
byte ix = 1;

15
src/Messages/TextMsg.cs
View File

@ -13,10 +13,6 @@ namespace RoboidControl {
/// </summary>
public const byte length = 2;
/// <summary>
/// The length of the text without the null terminator
/// </summary>
public byte textLength;
/// <summary>
/// The text
/// </summary>
public string text = "";
@ -26,27 +22,20 @@ namespace RoboidControl {
/// </summary>
/// <param name="text">The text to send</param>
public TextMsg(string text) {
this.textLength = (byte)text.Length;
this.text = text;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public TextMsg(byte[] buffer) : base(buffer) {
this.textLength = buffer[0];
this.text = System.Text.Encoding.UTF8.GetString(buffer, 1, this.textLength);
}
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;
#if DEBUG
System.Console.WriteLine($"Send TextMsg {this.textLength} {this.text}");
#endif
byte ix = 0;
buffer[ix++] = TextMsg.Id;
buffer[ix++] = this.textLength;
buffer[ix++] = (byte)this.text.Length;
for (int textIx = 0; textIx < this.text.Length; textIx++)
buffer[ix++] = (byte)this.text[textIx];
return ix;

3
src/Messages/ThingMsg.cs
View File

@ -77,9 +77,6 @@ namespace RoboidControl {
if (buffer.Length < ThingMsg.length)
return 0;
#if DEBUG
System.Console.WriteLine($"Send ThingMsg [{this.networkId}/{this.thingId}] {this.thingType} {this.parentId}");
#endif
byte ix = 0;
buffer[ix++] = ThingMsg.id;
buffer[ix++] = this.networkId;

10
src/Participant.cs
View File

@ -44,16 +44,6 @@ namespace RoboidControl {
/// </summary>
protected readonly List<Thing> things = new List<Thing>();
public virtual void Update(ulong currentTimeMS = 0) {
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing != null)
thing.Update(currentTimeMS, true);
}
}
/// <summary>
/// Get a thing with the given ids
/// </summary>

29
src/SiteServer.cs
View File

@ -7,7 +7,7 @@ namespace RoboidControl {
/// <summary>
/// A site server is a participant which provides a shared simulated environment
/// </summary>
public class SiteServer : ParticipantUDP {
public class SiteServer : LocalParticipant {
public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
@ -29,7 +29,7 @@ namespace RoboidControl {
new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
// Register<TouchSensor>(Thing.Type.TouchSensor);
Register<TouchSensor>(Thing.Type.TouchSensor);
}
/// <summary>
@ -42,12 +42,11 @@ namespace RoboidControl {
public override void Publish() {
}
protected override void Process(Participant sender, ParticipantMsg msg) {
base.Process(sender, msg);
//if (msg.networkId == 0) {
//Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
this.Send(sender, new NetworkIdMsg(sender.networkId));
//}
protected override void Process(Participant remoteParticipant, ParticipantMsg msg) {
if (msg.networkId == 0) {
Console.WriteLine($"{this.name} received New Participant -> {remoteParticipant.networkId}");
this.Send(remoteParticipant, new NetworkIdMsg(remoteParticipant.networkId));
}
}
protected override void Process(Participant sender, NetworkIdMsg msg) { }
@ -57,15 +56,15 @@ namespace RoboidControl {
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) {
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)

17
src/Thing.cs
View File

@ -49,12 +49,11 @@ namespace RoboidControl {
public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
this.owner = owner;
this.type = thingType;
if (this.owner != null)
this.owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
}
public Thing(Participant owner) : this(owner, Type.Undetermined) { }
public Thing(Participant owner) : this(owner, Type.Undetermined) {}
/// <summary>
/// Create a new thing for a participant
/// </summary>
@ -66,7 +65,7 @@ namespace RoboidControl {
/// Create a new thing without communication abilities
/// </summary>
/// <param name="thingType">The type of thing</param>
public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, invokeEvent) {
public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(LocalParticipant.Isolated(), thingType, invokeEvent) {
}
/// <summary>
@ -287,6 +286,10 @@ namespace RoboidControl {
}
}
/// <summary>
/// Event triggered when the orientation has changed
/// </summary>
//public event ChangeHandler OnOrientationChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated orientation
/// </summary>
public bool orientationUpdated = false;
@ -300,7 +303,7 @@ namespace RoboidControl {
set {
if (_linearVelocity != value) {
_linearVelocity = value;
linearVelocityUpdated = true;
hasLinearVelocity = _linearVelocity.distance != 0;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
@ -312,7 +315,7 @@ namespace RoboidControl {
/// <summary>
/// Boolean indicating the thing has an updated linear velocity
/// </summary>
public bool linearVelocityUpdated = false;
public bool hasLinearVelocity = false;
private Spherical _angularVelocity = Spherical.zero;
/// <summary>
@ -323,7 +326,7 @@ namespace RoboidControl {
set {
if (_angularVelocity != value) {
_angularVelocity = value;
angularVelocityUpdated = true;
hasAngularVelocity = _angularVelocity.distance != 0;
OnAngularVelocityChanged?.Invoke(_angularVelocity);
}
}
@ -335,7 +338,7 @@ namespace RoboidControl {
/// <summary>
/// Boolean indicating the thing has an updated angular velocity
/// </summary>
public bool angularVelocityUpdated = false;
public bool hasAngularVelocity = false;
#if UNITY_5_3_OR_NEWER
/// <summary>

2
src/Things/DifferentialDrive.cs
View File

@ -8,7 +8,7 @@ namespace RoboidControl {
public DifferentialDrive() { }
/// @brief Create a differential drive with networking support
/// @param participant The local participant
public DifferentialDrive(ParticipantUDP participant) : base(participant, Type.Undetermined) { }
public DifferentialDrive(LocalParticipant participant) : base(participant, Type.Undetermined) { }
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters

2
src/Things/TouchSensor.cs
View File

@ -26,7 +26,7 @@ namespace RoboidControl {
public TouchSensor(Thing parent) : base(parent) { }
public ParticipantUDP thisParticipant;
public LocalParticipant thisParticipant;
/// <summary>
/// Value which is true when the sensor is touching something, false otherwise