public static void ReceiveVectors()
{
if (FakePosition)
{
if (SWatch == null)
{
SWatch = Stopwatch.StartNew();
}
float Rad = 300;
float TS = 1000.0f;
Vector3 Pos = new Vector3((float)Math.Sin(SWatch.ElapsedMilliseconds / TS) * Rad, 100, (float)Math.Cos(SWatch.ElapsedMilliseconds / TS) * Rad);
MarkerA = MarkerB = MarkerC = Pos;
return;
}
byte[] Bytes = ReceiveRaw();
//Console.WriteLine(Bytes.Length);
Vector3 *Vectors = stackalloc Vector3[3];
Marshal.Copy(Bytes, 0, new IntPtr(Vectors), 3 * 3 * sizeof(float)); // Create vectors from bytes recieved over UDP
Vector3 A = ABuffer.PushGetAverage(Vectors[0] + Program.OptotrakOffset); // Method in CircularVectorBuffer class. This basically performs the running average filter
Vector3 B = BBuffer.PushGetAverage(Vectors[1] + Program.OptotrakOffset);
Vector3 C = CBuffer.PushGetAverage(Vectors[2] + Program.OptotrakOffset);
if (IsVisible(A)) // Checks for marker visibility. If it is not the last value is used.
{
MarkerA = A.YZX();
}
if (IsVisible(B))
{
MarkerB = B.YZX();
}
if (IsVisible(C))
{
MarkerC = C.YZX();
}
if (!FirstItemReceived)
{
FirstItemReceived = true;
Console.WriteLine(ConsoleColor.Yellow, "Receiving Optotrak data! {0}", GetPos());
}
}
public static void ReceiveVectors()
{
if (FakePosition)
{
if (SWatch == null)
{
SWatch = Stopwatch.StartNew();
}
float Rad = 300;
float TS = 1000.0f;
Vector3 Pos = new Vector3((float)Math.Sin(SWatch.ElapsedMilliseconds / TS) * Rad, 100, (float)Math.Cos(SWatch.ElapsedMilliseconds / TS) * Rad);
MarkerA = MarkerB = MarkerC = Pos;
return;
}
byte[] Bytes = ReceiveRaw();
//Console.WriteLine(Bytes.Length);
Vector3 *Vectors = stackalloc Vector3[3];
Marshal.Copy(Bytes, 0, new IntPtr(Vectors), 3 * 3 * sizeof(float));
Vector3 A = ABuffer.PushGetAverage(Vectors[0] + Program.OptotrakOffset);
Vector3 B = BBuffer.PushGetAverage(Vectors[1] + Program.OptotrakOffset);
Vector3 C = CBuffer.PushGetAverage(Vectors[2] + Program.OptotrakOffset);
if (IsVisible(A))
{
MarkerA = A.YZX();
}
if (IsVisible(B))
{
MarkerB = B.YZX();
}
if (IsVisible(C))
{
MarkerC = C.YZX();
}
if (!FirstItemReceived)
{
FirstItemReceived = true;
Console.WriteLine(ConsoleColor.Yellow, "Receiving Optotrak data! {0}", GetPos());
}
}