/// <summary>
/// 64B + 4B flag
/// </summary>
protected Matrix4x4?ReadTransformation(ReadBuffer reader)
{
var hasValue = reader.ReadInt32() == 1;
// 4 * 4 * sizeof(float)
int readOffset;
reader.ReserveForReading(64, out readOffset);
if (!hasValue)
{
return(null);
}
unsafe
{
var buffer = reader.GetBuffer();
fixed(byte *bufferPtr = buffer)
{
Matrix4x4 result;
byte * resultMatrixPtr = (byte *)&result;
DataManipulation.Copy(bufferPtr + readOffset, resultMatrixPtr, 64);
return(result);
}
}
}
public override bool ReadData(ReadBuffer reader)
{
// TODO; validate status
var operationStatus = (OperationStatus)reader.ReadInt32();
var systemTime = reader.ReadInt64();
var dataLenght = reader.ReadInt32();
var bodyData = new byte[dataLenght];
int readOffset;
reader.ReserveForReading(dataLenght, out readOffset);
unsafe
{
var buffer = reader.GetBuffer();
fixed(byte *bodyDataPtr = bodyData)
fixed(byte *bufferPtr = buffer)
{
DataManipulation.Copy(bufferPtr + readOffset, bodyDataPtr, (uint)dataLenght);
}
}
BodyFrame = BodyFrame.Parse(bodyData, TimeSpan.FromTicks(systemTime));
return(true);
}
public override bool WriteData(WriteBuffer writer)
{
if (_init)
{
_dataCapacity = Bitmap.PixelWidth * Bitmap.PixelHeight * 2;
writer.Write((int)OperationCode.DepthFrameTransfer);
writer.Write((int)OperationStatus.PushInit);
writer.Write((int)Bitmap.BitmapPixelFormat);
writer.Write(Bitmap.PixelWidth);
writer.Write(Bitmap.PixelHeight);
writer.Write(_dataCapacity);
// camera intrinsics
WriteIntrinsics(writer, CameraIntrinsics);
// write transformation
WriteTransformation(writer, DepthToColorTransform);
_init = false;
return(false);
}
writer.Write((int)OperationCode.DepthFrameTransfer);
writer.Write((int)OperationStatus.Push);
// just for check?
writer.Write(_offset);
// todo configurable chunks size support
// account for 4 bytes taken by chunkSize info!!
var dataChunkSize = Math.Min(_dataCapacity - _offset, writer.RemainingPacketWriteCapacity - 4);
writer.Write(dataChunkSize);
int writeOffset;
writer.ReserveForWrite(dataChunkSize, out writeOffset);
unsafe
{
using (var bitBuffer = Bitmap.LockBuffer(BitmapBufferAccessMode.Read))
using (var bufferRef = bitBuffer.CreateReference())
{
byte *dataSourcePtr;
uint bufferCapacity;
((IMemoryBufferByteAccess)bufferRef).GetBuffer(out dataSourcePtr, out bufferCapacity);
var buffer = writer.GetBuffer();
fixed(byte *bufferPtr = buffer)
{
DataManipulation.Copy(dataSourcePtr + _offset, bufferPtr + writeOffset, (uint)dataChunkSize);
}
}
}
_offset += dataChunkSize;
return(_offset == _dataCapacity);
}
public override bool ReadData(ReadBuffer reader)
{
if (Bitmap == null)
{
// header + CI + transformation mx
var status = (OperationStatus)reader.ReadInt32();
var pixelFormat = (BitmapPixelFormat)reader.ReadInt32();
var width = reader.ReadInt32();
var height = reader.ReadInt32();
var bitmapSize = reader.ReadInt32();
Bitmap = new SoftwareBitmap(pixelFormat, width, height, BitmapAlphaMode.Ignore);
CameraIntrinsics = ReadCameraIntrinsics(reader);
DepthToColorTransform = ReadTransformation(reader);
return(false);
}
var operationStatus = (OperationStatus)reader.ReadInt32();
// check?
var offset = reader.ReadInt32();
var dataLength = reader.ReadInt32();
int readOffset;
reader.ReserveForReading(dataLength, out readOffset);
uint bufferCapacity;
using (var buffer = Bitmap.LockBuffer(BitmapBufferAccessMode.Write))
using (var bufferRef = buffer.CreateReference())
{
unsafe
{
byte *targetPtr;
((IMemoryBufferByteAccess)bufferRef).GetBuffer(out targetPtr, out bufferCapacity);
targetPtr += _offset;
var readBuffer = reader.GetBuffer();
fixed(byte *readBufferPtr = readBuffer)
{
DataManipulation.Copy(readBufferPtr + readOffset, targetPtr, (uint)dataLength);
_offset += dataLength;
}
}
}
return(_offset == bufferCapacity);
}
public override bool WriteData(WriteBuffer writer)
{
if (_init)
{
writer.Write((int)OperationCode.UserFrameTransfer);
writer.Write((int)OperationStatus.PushInit);
writer.Write(Data.Length);
_init = false;
return(false);
}
writer.Write((int)OperationCode.UserFrameTransfer);
writer.Write((int)OperationStatus.Push);
// just for check?
writer.Write(_offset);
// todo configurable chunks size support
// account for 4 bytes taken by chunkSize info!!
var dataChunkSize = Math.Min(Data.Length - _offset, writer.RemainingPacketWriteCapacity - 4);
writer.Write(dataChunkSize);
int writeOffset;
writer.ReserveForWrite(dataChunkSize, out writeOffset);
unsafe
{
var data = Data;
var buffer = writer.GetBuffer();
fixed(byte *bufferPtr = buffer)
fixed(byte *dataSourcePtr = data)
{
DataManipulation.Copy(dataSourcePtr + _offset, bufferPtr + writeOffset, (uint)dataChunkSize);
}
}
_offset += dataChunkSize;
return(_offset == Data.Length);
}
/// <summary>
/// 64B + 4B flag
/// </summary>
protected void WriteTransformation(WriteBuffer writer, Matrix4x4?transformation)
{
writer.Write(transformation.HasValue ? 1 : 0);
// 4 * 4 * sizeof(float)
int writeOffset;
// todo: check for fail of reservation?
writer.ReserveForWrite(64, out writeOffset);
if (transformation.HasValue)
{
var transMatrix = transformation.Value;
var buffer = writer.GetBuffer();
unsafe
{
fixed(byte *bufferPtr = buffer)
{
byte *transformationPtr = (byte *)&transMatrix;
DataManipulation.Copy(transformationPtr, bufferPtr + writeOffset, 64);
}
}
}
}
public override bool ReadData(ReadBuffer reader)
{
if (Data == null)
{
// header
var status = (OperationStatus)reader.ReadInt32();
var dataSize = reader.ReadInt32();
Data = new byte[dataSize];
return(false);
}
var operationStatus = (OperationStatus)reader.ReadInt32();
// check?
var offset = reader.ReadInt32();
var dataLength = reader.ReadInt32();
int readOffset;
reader.ReserveForReading(dataLength, out readOffset);
unsafe
{
var data = Data;
var readBuffer = reader.GetBuffer();
fixed(byte *readBufferPtr = readBuffer)
fixed(byte *targetDataPtr = data)
{
DataManipulation.Copy(readBufferPtr + readOffset, targetDataPtr + _offset, (uint)dataLength);
_offset += dataLength;
}
}
return(_offset == Data.Length);
}
public override bool WriteData(WriteBuffer writer)
{
// frame packet generic header
writer.Write((int)OperationCode.BodyFrameTransfer);
writer.Write((int)OperationStatus.PushInit);
writer.Write(BodyFrame.SystemRelativeTime.Value.Ticks);
writer.Write(BodyFrame.BinaryData.Length);
int writeOffset;
writer.ReserveForWrite(BodyFrame.BinaryData.Length, out writeOffset);
unsafe
{
var buffer = writer.GetBuffer();
fixed(byte *bufferPtr = buffer)
fixed(byte *sourceDataPtr = BodyFrame.BinaryData)
{
DataManipulation.Copy(sourceDataPtr, bufferPtr + writeOffset, (uint)BodyFrame.BinaryData.Length);
}
}
return(true);
}