SHARE
TWEET

C# time warping sample code

a guest Apr 19th, 2014 708 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // All rights to this code sample waived under Creative Commons Zero Waiver
  2.  
  3. using System;
  4. using System.Collections.Generic;
  5. using System.IO;
  6. using System.Linq;
  7. using System.Text;
  8. using System.Threading.Tasks;
  9. using System.Drawing;
  10.  
  11. namespace timewarp
  12. {
  13.     class Program
  14.     {
  15.         static int imageWidth = 2560;
  16.         static int imageHeight = 1440;
  17.  
  18.         //static string filenamePrefix = "teapotwarped";
  19.         //static string depthFilename = "teapotdepthnoaa.raw";
  20.         //static string pixelsFilename = "teapot.raw";
  21.         //static double fovDegreesHorizontal = 45.0;
  22.         //static double fovDegreesVertical = 26.231;
  23.         //static double zscale = 1.5;
  24.         //static double minDepth = 93.0;
  25.         //static double maxDepth = 123.0;
  26.  
  27.         //static string filenamePrefix = "room";
  28.         //static string depthFilename = "roomdepthnoaa.raw";
  29.         //static string pixelsFilename = "room.raw";
  30.         //static double fovDegreesHorizontal = 110.0;
  31.         //static double fovDegreesVertical = 77.552;
  32.         //static double zscale = 1.0;
  33.         //static double minDepth = 60;
  34.         //static double maxDepth = 330.0;
  35.  
  36.         static string filenamePrefix = "planes1";
  37.         static string depthFilename = "planes1depthnoaa.raw";
  38.         static string pixelsFilename = "planes1.raw";
  39.         static double fovDegreesHorizontal = 106.26;
  40.         static double fovDegreesVertical = 73.74;
  41.         static double zscale = 1.0;
  42.         static double minDepth = 69.0;
  43.         static double maxDepth = 111.0;
  44.  
  45.         static void Main(string[] args)
  46.         {
  47.  
  48.             byte[] depthMapBytes = File.ReadAllBytes(depthFilename);
  49.             double[] depthMap = new double[depthMapBytes.Length/2];
  50.             for (int i=0; i < depthMap.Length; i++)
  51.             {
  52.                 ushort depthShort = (ushort)(depthMapBytes[2 * i] + (depthMapBytes[2 * i + 1] << 8));
  53.                 depthMap[i] = maxDepth - (maxDepth - minDepth)*depthShort/ushort.MaxValue;
  54.             }
  55.  
  56.             byte[] pixelRgbBytes = File.ReadAllBytes(pixelsFilename);
  57.             Color[] pixels = new Color[pixelRgbBytes.Length/3];
  58.             for (int i=0; i < pixels.Length; i++)
  59.             {
  60.                 pixels[i] = Color.FromArgb(255, pixelRgbBytes[3 * i],
  61.                                                 pixelRgbBytes[3 * i + 1],
  62.                                                 pixelRgbBytes[3 * i + 2]);
  63.             }
  64.             pixelRgbBytes = null;
  65.  
  66.             CreateRotatedImage(pixels, depthMap, 0, 0, filenamePrefix + ".shiftright.png", 0.0, 30.0);
  67.  
  68.             int counter = 1;
  69.             for (double degrees = -90; degrees <= 90; degrees+=0.5, counter++)
  70.             {
  71.                 //Console.WriteLine("Degrees: " + degrees);
  72.                 //CreateRotatedImage(pixels, depthMap, degrees, 0, filenamePrefix + "." + counter + ".rotateeye.png", 0.0, 0.0);
  73.                 //CreateRotatedImage(pixels, depthMap, 0, degrees, filenamePrefix + "." + counter + ".rotateeyey.png", 0.0, 0.0);
  74.                 //CreateRotatedImage(pixels, depthMap, degrees, 0, filenamePrefix + "." + counter + ".rotateneck.png", 15.0, 0.0);
  75.                 //CreateRotatedImage(pixels, depthMap, degrees, 0, filenamePrefix + "." + counter + ".rotatearound.png", -((minDepth + maxDepth) / 2), 0.0);
  76.             }
  77.         }
  78.  
  79.         private static void CreateRotatedImage(Color[] pixels, double[] depthMap, double rotateDegreesX, double rotateDegreesY, string filename, double zTranslate, double xTranslate)
  80.         {
  81.             double[] resultDepthMap = new double[depthMap.Length];
  82.             for (int i = 0; i < resultDepthMap.Length; i++)
  83.             {
  84.                 resultDepthMap[i] = double.PositiveInfinity;
  85.             }
  86.  
  87.             Color[] result = new Color[pixels.Length];
  88.             Bitmap bitmap = new Bitmap(imageWidth, imageHeight);
  89.             Graphics g = Graphics.FromImage(bitmap);
  90.             for (int i = 0; i < pixels.Length; i++)
  91.             {
  92.                 int xi = i % imageWidth;
  93.                 int yi = i / imageWidth;
  94.                 double x = xi;
  95.                 double y = yi;
  96.                 double newZ;
  97.                 double centerZ = depthMap[i];
  98.                 double[] zvalues = new double[4] { centerZ, centerZ, centerZ, centerZ };
  99.                 if (xi - 1 >= 0 && yi - 1 >= 0) { zvalues[0] = (centerZ + depthMap[(yi - 1) * imageWidth + (xi - 1)]) / 2; }
  100.                 if (xi + 1 < imageWidth && yi - 1 >= 0) { zvalues[1] = (centerZ + depthMap[(yi - 1) * imageWidth + (xi + 1)]) / 2; }
  101.                 if (xi + 1 < imageWidth && yi + 1 < imageHeight) { zvalues[2] = (centerZ + depthMap[(yi + 1) * imageWidth + (xi + 1)]) / 2; }
  102.                 if (xi - 1 >= 0 && yi + 1 < imageHeight) { zvalues[3] = (centerZ + depthMap[(yi + 1) * imageWidth + (xi - 1)]) / 2; }
  103.                 if (Math.Max(Math.Max(Math.Max(zvalues[0], zvalues[1]), zvalues[2]), zvalues[3]) - Math.Min(Math.Min(Math.Min(zvalues[0], zvalues[1]), zvalues[2]), zvalues[3]) > 1.0)
  104.                 {
  105.                     zvalues[0] = centerZ; zvalues[1] = centerZ; zvalues[2] = centerZ; zvalues[3] = centerZ;
  106.                 }
  107.  
  108.                 PointF vertex = TransformImagePoint(rotateDegreesX, rotateDegreesY, zTranslate, xTranslate, x, y, centerZ, out newZ);
  109. #if true
  110.                 PointF[] vertices = new PointF[4];
  111.                 double dummyZ;
  112.                 vertices[0] = TransformImagePoint(rotateDegreesX, rotateDegreesY, zTranslate, xTranslate, x - 0.5, y - 0.5, zvalues[0], out dummyZ);
  113.                 vertices[1] = TransformImagePoint(rotateDegreesX, rotateDegreesY, zTranslate, xTranslate, x + 0.5, y - 0.5, zvalues[1], out dummyZ);
  114.                 vertices[2] = TransformImagePoint(rotateDegreesX, rotateDegreesY, zTranslate, xTranslate, x + 0.5, y + 0.5, zvalues[2], out dummyZ);
  115.                 vertices[3] = TransformImagePoint(rotateDegreesX, rotateDegreesY, zTranslate, xTranslate, x - 0.5, y + 0.5, zvalues[3], out dummyZ);
  116. #endif
  117.  
  118.                 int depthMapIndex = (int)Math.Round(vertex.Y) * imageWidth + (int)Math.Round(vertex.X);
  119.  
  120.                 if (newZ >= 1 && vertex.X >= 0 && vertex.Y >= 0 && vertex.X < imageWidth - 0.5 && vertex.Y < imageHeight - 0.5 && newZ <= resultDepthMap[depthMapIndex])
  121.                 {
  122.                     bitmap.SetPixel((int)Math.Round(vertex.X), (int)Math.Round(vertex.Y), pixels[i]);
  123. #if true
  124.                     using (Brush brush = new SolidBrush(pixels[i]))
  125.                     {
  126.                         g.FillPolygon(brush, vertices);
  127.                         using (Pen pen = new Pen(pixels[i]))
  128.                         {
  129.                             g.DrawPolygon(pen, vertices);
  130.                         }
  131.                         g.FillRectangle(brush, new Rectangle((int)Math.Round(vertex.X) - 1, (int)Math.Round(vertex.Y) - 1, 2, 2));
  132.                     }
  133. #endif
  134.                     resultDepthMap[depthMapIndex] = newZ;
  135.                 }
  136.             }
  137.  
  138.             bitmap.Save(filename);
  139.             bitmap.Dispose();
  140.         }
  141.  
  142.         private static PointF TransformImagePoint(double rotateDegreesX, double rotateDegreesY, double zTranslate, double xTranslate, double x, double y, double z, out double newZ)
  143.         {
  144.             // z /= Math.Sqrt(1*1+Math.Sqrt(x*x+y*y));
  145.             ImageSpaceToWorldSpace(x, y, out x, out y);
  146.             TransformPoint(zTranslate, xTranslate, rotateDegreesX, rotateDegreesY, ref x, ref y, ref z);
  147.             WorldSpaceToImageSpace(ref x, ref y);
  148.             PointF transformedScreenPoint = new PointF((float)x, (float)y);
  149.             newZ = z;
  150.             return transformedScreenPoint;
  151.         }
  152.  
  153.         private static void WorldSpaceToImageSpace(ref double x, ref double y)
  154.         {
  155.             double scalex;
  156.             double scaley;
  157.             GetScaleFactors(out scalex, out scaley);
  158.             x = (x / scalex) * (imageWidth) + (imageWidth / 2);
  159.             y = (y / scaley) * (imageHeight) + (imageHeight / 2);
  160.         }
  161.  
  162.         private static void ImageSpaceToWorldSpace(double xin, double yin, out double x, out double y)
  163.         {
  164.             double scalex;
  165.             double scaley;
  166.             GetScaleFactors(out scalex, out scaley);
  167.             x = (xin - imageWidth / 2) / (imageWidth) * scalex;
  168.             y = (yin - imageHeight / 2) / (imageHeight) * scaley;
  169.         }
  170.  
  171.         private static void GetScaleFactors(out double scalex, out double scaley)
  172.         {
  173.             // At z=1 plane, image should be fovDegrees wide
  174.             scalex = 2 * Math.Tan(fovDegreesHorizontal * Math.PI / 180 / 2) * zscale;
  175.             scaley = 2 * Math.Tan(fovDegreesVertical * Math.PI / 180 / 2) * zscale;
  176.         }
  177.  
  178.         private static void TransformPoint(double zTranslate, double xTranslate, double rotateDegreesX, double rotateDegreesY, ref double x, ref double y, ref double z)
  179.         {
  180.             double sina, cosa;
  181.  
  182.             x *= z;
  183.             y *= z;
  184.             z += zTranslate;
  185.  
  186.             x -= xTranslate;
  187.  
  188.             if (rotateDegreesX != 0.0)
  189.             {
  190.                 sina = Math.Sin(rotateDegreesX * Math.PI / 180);
  191.                 cosa = Math.Cos(rotateDegreesX * Math.PI / 180);
  192.                 double xnew = x * cosa - z * sina;
  193.                 double znew = x * sina + z * cosa;
  194.                 x = xnew; z = znew;
  195.             }
  196.  
  197.             if (rotateDegreesY != 0.0)
  198.             {
  199.                 sina = Math.Sin(rotateDegreesY * Math.PI / 180);
  200.                 cosa = Math.Cos(rotateDegreesY * Math.PI / 180);
  201.                 double ynew = y * cosa - z * sina;
  202.                 double znew = y * sina + z * cosa;
  203.                 y = ynew; z = znew;
  204.             }
  205.  
  206.             z -= zTranslate;
  207.             x /= z;
  208.             y /= z;
  209.         }
  210.     }
  211. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top