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();
}
}