static void Draw(float Dt)
{
WorldSurface.Draw(Default);
if (Program.RenderVoxels)
{
ShaderUniforms.Model = Matrix4x4.CreateTranslation(-Program.WorldOrigin);
DefaultFlatColor.Bind();
Voxels.VoxMesh.Draw();
DefaultFlatColor.Unbind();
}
Matrix4x4 TransRot = OptotrakClient.GetRotation() * OptotrakClient.GetTranslation(); // This is the transformation matrix for rendering of the plane
ShaderUniforms.Model = Matrix4x4.CreateScale(10) * TransRot; // Translates, rotates and scales the plane to appropriate size.
Default.Bind();
Plane.Draw();
Default.Unbind();
if (Program.RenderPoints)
{
RealSenseClient.GetVerts(ref Points);
ShaderUniforms.Model = Matrix4x4.Identity;
DefaultFlatColor.Bind();
Points.Draw();
DefaultFlatColor.Unbind();
}
DrawPin(PinMat1, LegClient.R_Start - Program.WorldOrigin);
DrawPin(PinMat2, LegClient.R_End - Program.WorldOrigin);
DrawPin(PinMat1, LegClient.L_Start - Program.WorldOrigin);
DrawPin(PinMat2, LegClient.L_End - Program.WorldOrigin);
}
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);
}