static Vertex3[] OnPointCloud(int Count, Vertex3[] Verts, FrameData[] Frames)
{
if (Verts == null)
{
if (VertsArr == null || VertsArr.Length < Count)
{
VertsArr = new Vertex3[Count];
lock (Lck)
Points = new Vertex3[Count];
}
return(VertsArr);
}
FrameData Clr = Frames[1];
lock (Lck) { // thread synchronization stuff. Used to lock an object to prevent other threads from chaging it... (Monitor.Enter)
Vector3 CamPos = OptotrakClient.GetPos();
OptotrakClient.GetRotationAngles(out float Yaw, out float Pitch, out float Roll);
Matrix4x4 TransMat = Matrix4x4.CreateFromYawPitchRoll(Yaw, Pitch + (float)Math.PI, -Roll) * OptotrakClient.GetTranslation();
PointCount = 0;
fixed(Vertex3 *PointsPtr = Points)
Util.MemSet(new IntPtr(PointsPtr), 0, Points.Length * sizeof(Vertex3));
//Voxels.SetVoxel(0, 0, 0, new VoxelEntry(VoxelType.Solid));
//Voxels.Fill(new VoxelEntry(VoxelType.Solid));
Voxels.Fill(VoxelEntry.Empty);
for (int i = 0, j = 0; i < Count; i++) // This loops trough all the points in the point cloud
{
if (Verts[i].Position == Vector3.Zero) // If a point position in 0 discard it.
{
continue;
}
Vertex3 Vert = Verts[i];
Vert.Position = Vector3.Transform((Vert.Position * 1000), TransMat); // Transfomr meter into mm and transforms point from camera space to world space (coordinate frame)
if (Vector3.Distance(CamPos, Vert.Position) < 500) // If a point is closer than 500 mm discard it. Because the camera does not work well with points so close.
{
continue;
}
Vert.Color = Clr.GetPixel(Vert.UV, false); // Gets color and puts it into the vertex
Points[j++] = Vert;
PointCount++;
Vector3 WorldPos = Vert.Position + Program.WorldOrigin; // Align graphics coordinate system to world coordinate system....
/*Voxels.Cast(CamPos, WorldPos, (X, Y, Z) => {
* Voxels.SetVoxel(X, Y, Z, new VoxelEntry(VoxelType.None));
* });*/
Voxels.SetVoxel(WorldPos, new VoxelEntry(VoxelType.Solid, Vert.Color)); // If any vertx is in the volume of voxels, set the corresponding voxel as solid and the same color as the vertex.
}
//This is the "Collision detection". The function Voxels.Ray takes in 2 position arguments and a function as an argument (the (X,Y,Z)... part). Search for delegate :)
RightLegCollides = !Voxels.Ray(LegClient.R_Start, LegClient.R_End, (X, Y, Z) => {
// The part inside this braces gets called multiple times, once for every voxel between the start and end position
if (Voxels.IsSolid(X, Y, Z)) // Checks for actual collision, if there is a solid block at this point, return false, which stops the .Ray function.
{
return(false);
}
Voxels.SetVoxel(X, Y, Z, new VoxelEntry(VoxelType.NonSolid, RightLegCollides ? Color.Red : Color.Green)); // Based on the value of RightLegCollides, color the voxels green or red. The red voxels actually render one frame after the colision was detected.
return(true);
});
LeftLegCollides = !Voxels.Ray(LegClient.L_Start, LegClient.L_End, (X, Y, Z) => {
if (Voxels.IsSolid(X, Y, Z))
{
return(false);
}
Voxels.SetVoxel(X, Y, Z, new VoxelEntry(VoxelType.NonSolid, LeftLegCollides ? Color.Red : Color.Green));
return(true);
});
if (CollisionSenderSocket == null)
{
CollisionSenderSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
}
IPEndPoint EndPoint = new IPEndPoint(IPAddress.Parse(Program.CollisionReceiverAddress.Split(':')[0]), int.Parse(Program.CollisionReceiverAddress.Split(':')[1]));
CollisionSenderSocket.SendTo(new byte[] { ToByte(LeftLegCollides), ToByte(RightLegCollides) }, EndPoint);
if (!Program.MarkDirtyAuto)
{
Voxels.MarkDirty();
}
}
return(null);
}
static Vector3 Transform(Vector3 In) //This attaches the legs to the camera (in the world coordinates)
{
return(In.YZX() + OptotrakClient.GetPos() - Program.CameraHeight + Program.WorldOrigin);
}
public static void Init(float TargetFramerate, float Treadmill_X, float Treadmill_Z)
{
RenderWindow.Treadmill_X = Treadmill_X;
RenderWindow.Treadmill_Z = Treadmill_Z;
Framerate = TargetFramerate;
Console.WriteLine(ConsoleColor.Green, "Target framerate: {0} FPS", TargetFramerate);
Console.WriteLine(ConsoleColor.Green, "M - Camera position");
Console.WriteLine(ConsoleColor.Green, "F1 - Toggle RealSense capture");
Console.WriteLine(ConsoleColor.Green, "F2 - Toggle mouse capture");
Console.WriteLine(ConsoleColor.Green, "F3 - Toggle render points");
Console.WriteLine(ConsoleColor.Green, "F4 - Toggle render voxels");
Console.WriteLine(ConsoleColor.Green, "F5 - Move camera to RealSense position");
Vector2 Res = RWnd.GetDesktopResolution() * 0.9f;
RWnd = new RWnd((int)Res.X, (int)Res.Y, nameof(ColliderGUI)); // Create the window of the GUI
Console.WriteLine(ConsoleColor.Green, "Running {0}", RenderAPI.Version);
Console.WriteLine(ConsoleColor.Green, RenderAPI.Renderer);
Console.LogWriteLine("OpenGL Extensions:\n {0}", string.Join("\n ", RenderAPI.Extensions));
CaptureMouse = false;
RWnd.OnMouseMoveDelta += (Wnd, X, Y) => {
if (CaptureMouse)
{
Cam.Update(-new Vector2(X, Y));
}
};
RWnd.OnKey += (RWnd Wnd, Key Key, int Scancode, bool Pressed, bool Repeat, KeyMods Mods) => {
if (Key == Key.Space)
{
MoveVec.Y = Pressed ? 1 : 0;
}
else if (Key == Key.C)
{
MoveVec.Y = Pressed ? -1 : 0;
}
else if (Key == Key.W)
{
MoveVec.Z = Pressed ? -1 : 0;
}
else if (Key == Key.A)
{
MoveVec.X = Pressed ? -1 : 0;
}
else if (Key == Key.S)
{
MoveVec.Z = Pressed ? 1 : 0;
}
else if (Key == Key.D)
{
MoveVec.X = Pressed ? 1 : 0;
}
if (Pressed && Key == Key.Escape)
{
RWnd.Close();
}
if (Pressed && Key == Key.M)
{
Console.WriteLine(ConsoleColor.Green, "Pos: {0}", Cam.Position);
}
if (Pressed && Key == Key.F1)
{
Program.RealSenseEnabled = !Program.RealSenseEnabled;
}
if (Pressed && Key == Key.F2)
{
CaptureMouse = !CaptureMouse;
}
if (Pressed && Key == Key.F3)
{
Program.RenderPoints = !Program.RenderPoints;
}
if (Pressed && Key == Key.F4)
{
Program.RenderVoxels = !Program.RenderVoxels;
}
if (Pressed && Key == Key.F5)
{
Cam.Position = OptotrakClient.GetPos();
}
if (Key == Key.MouseLeft)
{
CaptureMouse = Pressed;
}
};
SetupCamera();
LoadAssets();
Gfx.PointSize(5); // Point size of the verteces
}
static Vertex3[] OnPointCloud(int Count, Vertex3[] Verts, FrameData[] Frames)
{
if (Verts == null)
{
if (VertsArr == null || VertsArr.Length < Count)
{
VertsArr = new Vertex3[Count];
lock (Lck)
Points = new Vertex3[Count];
}
return(VertsArr);
}
FrameData Clr = Frames[1];
lock (Lck) {
Vector3 CamPos = OptotrakClient.GetPos();
OptotrakClient.GetRotationAngles(out float Yaw, out float Pitch, out float Roll);
Matrix4x4 TransMat = Matrix4x4.CreateFromYawPitchRoll(Yaw, Pitch + (float)Math.PI, -Roll) * OptotrakClient.GetTranslation();
PointCount = 0;
fixed(Vertex3 *PointsPtr = Points)
Util.MemSet(new IntPtr(PointsPtr), 0, Points.Length * sizeof(Vertex3));
//Voxels.SetVoxel(0, 0, 0, new VoxelEntry(VoxelType.Solid));
//Voxels.Fill(new VoxelEntry(VoxelType.Solid));
Voxels.Fill(VoxelEntry.Empty);
for (int i = 0, j = 0; i < Count; i++)
{
if (Verts[i].Position == Vector3.Zero)
{
continue;
}
Vertex3 Vert = Verts[i];
Vert.Position = Vector3.Transform((Vert.Position * 1000), TransMat);
if (Vector3.Distance(CamPos, Vert.Position) < 500)
{
continue;
}
Vert.Color = Clr.GetPixel(Vert.UV, false);
Points[j++] = Vert;
PointCount++;
Vector3 WorldPos = Vert.Position + Program.WorldOrigin;
/*Voxels.Cast(CamPos, WorldPos, (X, Y, Z) => {
* Voxels.SetVoxel(X, Y, Z, new VoxelEntry(VoxelType.None));
* });*/
Voxels.SetVoxel(WorldPos, new VoxelEntry(VoxelType.Solid, Vert.Color));
}
RightLegCollides = !Voxels.Ray(LegClient.R_Start, LegClient.R_End, (X, Y, Z) => {
if (Voxels.IsSolid(X, Y, Z))
{
return(false);
}
Voxels.SetVoxel(X, Y, Z, new VoxelEntry(VoxelType.NonSolid, RightLegCollides ? Color.Red : Color.Green));
return(true);
});
LeftLegCollides = !Voxels.Ray(LegClient.L_Start, LegClient.L_End, (X, Y, Z) => {
if (Voxels.IsSolid(X, Y, Z))
{
return(false);
}
Voxels.SetVoxel(X, Y, Z, new VoxelEntry(VoxelType.NonSolid, LeftLegCollides ? Color.Red : Color.Green));
return(true);
});
if (!Program.MarkDirtyAuto)
{
Voxels.MarkDirty();
}
}
return(null);
}
static Vector3 Transform(Vector3 In)
{
return(In.YZX() + OptotrakClient.GetPos() - Program.CameraHeight + Program.WorldOrigin);
}