Improved coloring and output by taking into consideration the amplitude.

MariCam/Program.cs

@@ -62,6 +62,9 @@ namespace MariCam
         [System.Runtime.InteropServices.DllImport(LibraryPath)]
         public static extern IntPtr arducamCameraGetDepthData(IntPtr frameBuffer);
 
+        [System.Runtime.InteropServices.DllImport(LibraryPath)]
+        public static extern IntPtr arducamCameraGetAmplitudeData(IntPtr frameBuffer);
+
         public static unsafe void Main(string[] args)
         {
             Console.WriteLine("Setting up Camera");
@@ -88,7 +91,9 @@ namespace MariCam
                 Color.FromArgb(255, 255, 255, 0), //Yellow
                 Color.FromArgb(255, 255, 165, 0), //Orange
                 Color.FromArgb(255, 255, 0, 0) //Red
-            }, 255);
+            }, 15);
+
+            gradients.Reverse();
 
             var form = new Form();
 
@@ -114,37 +119,40 @@ namespace MariCam
 
                 float* depthData = (float*)arducamCameraGetDepthData(frameBuffer);
 
-                float min = float.MaxValue;
+                float* amplitudeData = (float*)arducamCameraGetAmplitudeData(frameBuffer);
-                float max = float.MinValue;
 
-                for (int y = 0; y < format.Height; y++)
+                uint min = 0;
-                {
+                uint max = 255;
-                    for (int x = 0; x < format.Width; x++)
+                int width = format.Width;
-                    {
+                int height = format.Height;
-                        var depth = *(depthData + (y * format.Width) + x);
-
-                        if (depth < min)
-                            min = depth;
-
-                        if (depth > max)
-                            max = depth;
-                    }
-                }
 
                 var bufferData = buffer.LockBits(new Rectangle(0, 0, buffer.Width, buffer.Height), ImageLockMode.ReadWrite, buffer.PixelFormat);
 
                 var bufferPixels = (uint*)bufferData.Scan0;
 
-                for (int y = 0; y < format.Height; y++)
+                for (int y = 0; y < height; y++)
                 {
-                    for (int x = 0; x < format.Width; x++)
+                    for (int x = 0; x < width; x++)
                     {
-                        var depth = *(depthData + (y * format.Width) + x);
+                        var depth = *(depthData + (y * width) + x);
 
-                        //maxNew − minNew / maxOld − minOld * (v − minOld) + minNew
+                        var amplitude = *(amplitudeData + (y * width) + x);
-                        var color = gradients[(int)(((float)((gradients.Count - 1) - 0) / (float)(max - min) * (depth - min)) + 0)];
 
-                        *(bufferPixels + ((y * format.Width) + x)) = ((uint)255 << 24) | ((uint)(color.R) << 16) | ((uint)(color.G) << 8) | (byte)(color.B);
+                        uint intensity = (uint)((1 - (depth / 2)) * 255) & (uint)(amplitude > 30 ? 255 : 0);
+
+                        Color color;
+
+                        if (intensity == min || intensity == max)
+                        {
+                            color = Color.Black;
+                        }
+                        else
+                        {
+                            //maxNew − minNew / maxOld − minOld * (v − minOld) + minNew
+                            color = gradients[(int)(((float)((gradients.Count - 1) - 0) / (float)(max - min) * (intensity - min)) + 0)];
+                        }
+
+                        *(bufferPixels + ((y * width) + x)) = ((uint)255 << 24) | ((uint)(color.R) << 16) | ((uint)(color.G) << 8) | (byte)(color.B);
                     }
                 }