static void Main(string[] Args)
{
LoadConfig();
int VoxelsX = (int)(Treadmill_X / VoxelSize);
int VoxelsY = (int)(Treadmill_Y / VoxelSize);
int VoxelsZ = (int)(Treadmill_Z / VoxelSize);
WorldOrigin = new Vector3(Treadmill_X, 0, 0);
CameraHeight = new Vector3(0, PelvisHeight, 0);
Console.Spawn(Args.Contains("--console") || SpawnConsole);
OptotrakClient.Init(40023);
LegClient.Init(40024);
RenderWindow.Init(60, Treadmill_X, Treadmill_Z);
Voxels.Init(VoxelsX, VoxelsY, VoxelsZ, VoxelSize);
RealSenseClient.Init();
while (RenderWindow.Tick())
{
// Additional logic which should execute per-frame goes here
if (!UseThreading)
{
RealSenseClient.Loop();
}
}
}
static void Main(string[] Args)
{
LoadConfig();
int VoxelsX = (int)(Treadmill_X / VoxelSize);
int VoxelsY = (int)(Treadmill_Y / VoxelSize);
int VoxelsZ = (int)(Treadmill_Z / VoxelSize);
WorldOrigin = new Vector3(Treadmill_X, 0, 0); // Shifts the world origin to the left side (the actual render window 0,0,0 is in the right corner)
CameraHeight = new Vector3(0, PelvisHeight, 0); // the distance from the floor to the camera
Console.Spawn(Args.Contains("--console") || SpawnConsole);
OptotrakClient.Init(40023); // initializes optotrack client and starts a thread for it. Starts also a thread for UDP (on port 40023)
LegClient.Init(40024); // The same as above but for MVN Analyze
RenderWindow.Init(60, Treadmill_X, Treadmill_Z);
Voxels.Init(VoxelsX, VoxelsY, VoxelsZ, VoxelSize);
RealSenseClient.Init();
while (RenderWindow.Tick())
{
// Additional logic which should execute per-frame goes here
if (!UseThreading)
{
RealSenseClient.Loop();
}
}
}
static void Update(float Dt)
{
const float MoveSpeed = 500;
if (!(MoveVec.X == 0 && MoveVec.Y == 0 && MoveVec.Z == 0))
{
Cam.Position += Cam.ToWorldNormal(Vector3.Normalize(MoveVec)) * MoveSpeed * Dt;
}
if (Program.RenderVoxels)
{
Voxels.Update();
}
}
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 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);
}
