public void DrawSonarFrame(Transform2D transform, PrimitiveBatch primitiveBatch) { var sonarFrame = SonarFrame; if (sonarFrame != null) { primitiveBatch.Begin(PrimitiveType.LineList); for (int i = 0; i < sonarFrame.Length; i++) { var measurement = sonarFrame[i] / 100f; var obstacle = new Vector2( SonarTransforms[i].Position.X + measurement * (float)Math.Cos(SonarTransforms[i].Rotation), SonarTransforms[i].Position.Y + measurement * (float)Math.Sin(SonarTransforms[i].Rotation)); var sonarStart = SonarTransforms[i].Position.Rotate(transform.Rotation) + transform.Position; obstacle = obstacle.Rotate(transform.Rotation) + transform.Position; sonarStart *= Constants.PixelsPerWorldUnit * new Vector2(1, -1); obstacle *= Constants.PixelsPerWorldUnit * new Vector2(1, -1); //var offset = 4 * Vector2.UnitY.Rotate(transform.Rotation); //var offset = Vector2.Normalize((obstacle - sonarStart).Rotate(MathHelper.PiOver2)); //primitiveBatch.AddVertex(sonarStart, Color.Red); //primitiveBatch.AddVertex(obstacle, Color.Red); //primitiveBatch.AddVertex(obstacle + offset, Color.Red); //primitiveBatch.AddVertex(obstacle + offset, Color.Red); //primitiveBatch.AddVertex(sonarStart + offset, Color.Red); //primitiveBatch.AddVertex(sonarStart, Color.Red); primitiveBatch.AddVertex(sonarStart, Color.Red); primitiveBatch.AddVertex(obstacle, Color.Red); } primitiveBatch.End(); } }
public void DrawSteeringVector(Transform2D transform, PrimitiveBatch primitiveBatch) { primitiveBatch.Begin(PrimitiveType.LineList); primitiveBatch.AddVertex(Constants.PixelsPerWorldUnit * new Vector2(1, -1) * transform.Position, Color.Green); primitiveBatch.AddVertex(Constants.PixelsPerWorldUnit * new Vector2(1, -1) * (Steering + transform.Position), Color.Green); primitiveBatch.End(); }
public void DrawKinectDepthMap(Transform2D transform, PrimitiveBatch primitiveBatch) { if (Frame != null) { var depthImage = Frame.DepthImage; var viewport = new Viewport(0, 0, Frame.Sensor.DepthStream.FrameWidth, Frame.Sensor.DepthStream.FrameHeight); primitiveBatch.Begin(PrimitiveType.TriangleList); for (int i = viewport.Height - 1; i >= 0; i -= DepthStep) { for (int j = 0; j < viewport.Width; j += DepthStep) { var depth = ((ushort)depthImage[i * viewport.Width + j]) >> 3; if (depth > 0) { var ray = CalculateCursorRay(viewport, new Vector2(j, i), DepthProjectionMatrix, Matrix.CreateLookAt(Vector3.Zero, -Vector3.UnitZ, Vector3.Up)); var obstacle = (depth / 1000f) * new Vector2(-ray.Direction.Z, ray.Direction.X); obstacle = obstacle.Rotate(transform.Rotation) + transform.Position; obstacle *= Constants.PixelsPerWorldUnit * new Vector2(1, -1); var color = GraphicsHelper.HsvToRgb(new Vector3(i, 1, 1)); primitiveBatch.AddVertex(obstacle, color); primitiveBatch.AddVertex(obstacle + DepthStep * Vector2.UnitX, color); primitiveBatch.AddVertex(obstacle + DepthStep * Vector2.UnitY, color); } } } primitiveBatch.End(); } }
public void DrawFreeSpace(Transform2D transform, PrimitiveBatch primitiveBatch) { if (FreeSpace != null) { var sizeX = new Vector2(2, 0); var sizeY = new Vector2(0, 2); primitiveBatch.Begin(PrimitiveType.TriangleList); for (int i = 0; i < FreeSpace.Length; i++) { var color = FreeSpace[i] ? Color.Green : Color.Red; primitiveBatch.AddVertex(transform.Position + i * sizeX, color); primitiveBatch.AddVertex(transform.Position + i * sizeX + sizeX, color); primitiveBatch.AddVertex(transform.Position + i * sizeX + sizeX + sizeY, color); primitiveBatch.AddVertex(transform.Position + i * sizeX + sizeX + sizeY, color); primitiveBatch.AddVertex(transform.Position + i * sizeX + sizeY, color); primitiveBatch.AddVertex(transform.Position + i * sizeX, color); } primitiveBatch.End(); } }